diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index 9a0984b7d758758bcf0fbff384f1916f6aadb3b8..e90fbf0aca2f7eb6d5bf8ddb55388780a9c6e94a 100644
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
@@ -133,272 +133,306 @@ public:
*/
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;
+
+ /**
+ * This method is used to reset all HighSensitive areas
+ */
+ void resetHighSensitive();
+
+ /**
+ * @return Probability of pixel p being occupied.
+ */
+ float getOccupancyProbability(Eigen::Vector2i p);
+
+ /**
+ * @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,
+ tf::Transform transform);
+
+ void insertRanges(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);
- /**
- * 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;
-
- /**
- * This method is used to reset all HighSensitive areas
- */
- void resetHighSensitive();
-
- /**
- * @return Probability of pixel p being occupied.
- */
- float getOccupancyProbability(Eigen::Vector2i p);
-
- /**
- * @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, tf::Transform transform);
-
- void insertRanges( 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 );
-
- /**
- * 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( Pose robotPose, std::vector<MeasurePoint> measurePoints );
-
- /**
- * @return QImage of size m_metaData.width, m_metaData.height with values of m_OccupancyProbability scaled to 0-254
- */
- QImage getProbabilityQImage(int trancparencyThreshold, bool showInaccessible) const;
-
- //puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
-
- /**
- * Returns an "image" of the obstacles e.g. seen in the 3D scans
- * @returns image with dark red dots in areas where the obstacles were seen
- */
- //puma2::ColorImageRGB8* getObstacleImage ( ); TODO
-
- /**
- * 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);
-
- /**
- * 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);
-
- /**
- * @brief This method computes the sharpness of the occupancy grid
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the map
- */
- double evaluateByContrast();
-
- /// GETTERS
-
- Box2D<int> getExploredRegion() { return m_ExploredRegion; }
- Box2D<int> getChangedRegion() { return m_ChangedRegion; }
-
- /**
- * Sets cells of this map to free or occupied according to maskMap
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg);
-
- void changeMapSize(int x_add_left, int y_add_up, int x_add_right, int y_add_down );
-
-
- protected:
-
- /**
- * This method increments m_MeasurementCount for pixel p.
- * @param p Pixel that has been measured.
- */
- void incrementMeasurementCount(Eigen::Vector2i p);
-
- /**
- * This method increments the occupancy count int m_OccupancyCount for pixel p.
- * @param p Occupied pixel.
- */
- void incrementOccupancyCount(Eigen::Vector2i p);
-
- /**
- * This method increments m_MeasurementCount and if neccessary m_OccupancyCount for all pixels.
- */
- void applyChanges();
-
- void clearChanges();
-
- /**
- * 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.
- */
- template<class DataT>
- void drawLine(DataT* data, 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);
-
- /**
- * 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.
- */
- void maximizeChangedRegion();
-
- /**
- * This method resets all values of m_ExploredX, m_MaxExploredX, m_MinExploredY and m_MaxExploredY.
- */
- void resetExploredRegion();
-
- /**
- * Deletes all allocated members.
- */
- void cleanUp();
-
- /**
- * 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;
-
- /**
- * 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;
-
- //bool to reset high_sensitive Areas on the next iteration
- bool m_reset_high;
-
- /**
- * 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;
-
- /**
- * ros transform listener
- */
- tf::TransformListener m_tfListener;
-
- /**
- * ros transformation laser to base_link
- */
- tf::StampedTransform m_laserTransform;
- tf::Transform m_latestMapTransform;
+ /**
+ * 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(Pose robotPose, std::vector<MeasurePoint> measurePoints);
+
+ /**
+ * @return QImage of size m_metaData.width, m_metaData.height with values of
+ * m_OccupancyProbability scaled to 0-254
+ */
+ QImage getProbabilityQImage(int trancparencyThreshold,
+ bool showInaccessible) const;
+
+ // puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
+
+ /**
+ * Returns an "image" of the obstacles e.g. seen in the 3D scans
+ * @returns image with dark red dots in areas where the obstacles were seen
+ */
+ // puma2::ColorImageRGB8* getObstacleImage ( ); TODO
+
+ /**
+ * 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);
+
+ /**
+ * 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);
+
+ /**
+ * @brief This method computes the sharpness of the occupancy grid
+ * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the map
+ */
+ double evaluateByContrast();
+
+ /// GETTERS
+
+ Box2D<int> getExploredRegion()
+ {
+ return m_ExploredRegion;
+ }
+ Box2D<int> getChangedRegion()
+ {
+ return m_ChangedRegion;
+ }
+
+ /**
+ * Sets cells of this map to free or occupied according to maskMap
+ */
+ void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+
+ void changeMapSize(int x_add_left, int y_add_up, int x_add_right,
+ int y_add_down);
+
+protected:
+ /**
+ * This method increments m_MeasurementCount for pixel p.
+ * @param p Pixel that has been measured.
+ */
+ void incrementMeasurementCount(Eigen::Vector2i p);
+ /**
+ * This method increments the occupancy count int m_OccupancyCount for pixel
+ * p.
+ * @param p Occupied pixel.
+ */
+ void incrementOccupancyCount(Eigen::Vector2i p);
+
+ /**
+ * This method increments m_MeasurementCount and if neccessary
+ * m_OccupancyCount for all pixels.
+ */
+ void applyChanges();
+
+ void clearChanges();
+
+ /**
+ * 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.
+ */
+ template <class DataT>
+ void drawLine(DataT* data, 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);
+
+ /**
+ * 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.
+ */
+ void maximizeChangedRegion();
+
+ /**
+ * This method resets all values of m_ExploredX, m_MaxExploredX,
+ * m_MinExploredY and m_MaxExploredY.
+ */
+ void resetExploredRegion();
+
+ /**
+ * Deletes all allocated members.
+ */
+ void cleanUp();
+
+ /**
+ * 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;
+
+ /**
+ * 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;
+
+ // bool to reset high_sensitive Areas on the next iteration
+ bool m_reset_high;
+
+ /**
+ * 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;
+
+ /**
+ * ros transform listener
+ */
+ tf::TransformListener m_tfListener;
+
+ /**
+ * ros transformation laser to base_link
+ */
+ tf::StampedTransform m_laserTransform;
+ tf::Transform m_latestMapTransform;
};
#endif
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
index be7f874b3c12dac101c93534d56373243d0b880c..47dfb6b67c47dd236005993e9076a2f3a0e92915 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
@@ -41,126 +41,132 @@ public:
*/
HyperSlamFilter(int particleFilterNum, int particleNum);
- 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)
- {
- m_SlamFilters[i]->applyMasking(msg);
- }
- }
+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);
- private:
+ /**
+ * The destructor releases the OccupancyMap and the particles.
+ */
+ ~HyperSlamFilter();
- /** Used SlamFilters */
- std::vector <SlamFilter*> m_SlamFilters;
+ /**
+ * 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);
- /** Number of SlamFilters */
- unsigned m_ParticleFilterNum;
+ /**
+ * Computes and sets the new value for m_Alpha1.
+ * @param percent Rotation error while rotating (see class constructor for
+ * details)
+ */
+ void setRotationErrorRotating(float percent);
- /** Number of Particles of SlamFilter */
- unsigned m_ParticleNum;
+ /**
+ * Computes and sets the new value for m_Alpha2.
+ * @param degreesPerMeter Rotation error while translating (see class
+ * constructor for details)
+ */
+ void setRotationErrorTranslating(float degreesPerMeter);
- /** */
- double m_DeletionThreshold;
+ /**
+ * Computes and sets the new value for m_Alpha3.
+ * @param percent Translation error while translating (see class constructor
+ * for details)
+ */
+ void setTranslationErrorTranslating(float percent);
- /** Best SLAM Filter */
- SlamFilter* m_BestSlamFilter;
+ /**
+ * Computes and sets the new value for m_Alpha4.
+ * @param mPerDegree Translation error while rotating (see class constructor
+ * for details)
+ */
+ void setTranslationErrorRotating(float mPerDegree);
- /** Worst SlamFilter */
- SlamFilter* m_WorstSlamFilter;
+ /**
+ * 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);
- bool m_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)
+ {
+ m_SlamFilters[i]->applyMasking(msg);
+ }
+ }
+
+private:
+ /** Used SlamFilters */
+ std::vector<SlamFilter*> m_SlamFilters;
+
+ /** Number of SlamFilters */
+ unsigned m_ParticleFilterNum;
+
+ /** Number of Particles of SlamFilter */
+ unsigned m_ParticleNum;
+
+ /** */
+ double m_DeletionThreshold;
+
+ /** Best SLAM Filter */
+ SlamFilter* m_BestSlamFilter;
+
+ /** Worst SlamFilter */
+ SlamFilter* m_WorstSlamFilter;
+
+ bool m_DoMapping;
};
#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
index db2aab8b2baa83d2bf4733f1c103ab54525bcca7..a9ab121b73136fba405304be68987e321f125e40 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
@@ -1,12 +1,11 @@
#ifndef PARTICLEFILTER_H
#define PARTICLEFILTER_H
-
+#include <homer_nav_libs/Math/Transformation2D.h>
#include <limits.h>
#include <omp.h>
-#include <cmath>
-#include <homer_nav_libs/Math/Transformation2D.h>
#include <sensor_msgs/LaserScan.h>
+#include <cmath>
#include <iostream>
#include <ros/ros.h>
@@ -33,297 +32,328 @@ const float MIN_EFFECTIVE_PARTICLE_WEIGHT = 0.2;
* @see Particle
*/
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;
-
- 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;
+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;
+
+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;
};
template <class ParticleType>
-ParticleFilter<ParticleType>::ParticleFilter(int particleNum) {
- // initialize particle lists
- m_CurrentList = new ParticleType*[particleNum];
- m_LastList = new ParticleType*[particleNum];
+ParticleFilter<ParticleType>::ParticleFilter(int particleNum)
+{
+ // initialize particle lists
+ m_CurrentList = new ParticleType*[particleNum];
+ m_LastList = new ParticleType*[particleNum];
- // initialize random number generator
- random01(time(0));
+ // initialize random number generator
+ random01(time(0));
- m_ParticleNum = particleNum;
+ m_ParticleNum = particleNum;
}
template <class ParticleType>
-ParticleFilter<ParticleType>::~ParticleFilter() {
- if (m_CurrentList) {
- delete[] m_CurrentList;
- m_CurrentList = 0;
- }
- if (m_LastList) {
- delete[] m_LastList;
- m_LastList = 0;
- }
+ParticleFilter<ParticleType>::~ParticleFilter()
+{
+ if (m_CurrentList)
+ {
+ delete[] m_CurrentList;
+ m_CurrentList = 0;
+ }
+ if (m_LastList)
+ {
+ delete[] m_LastList;
+ m_LastList = 0;
+ }
}
template <class ParticleType>
-int ParticleFilter<ParticleType>::getParticleNum() {
- return m_ParticleNum;
+int ParticleFilter<ParticleType>::getParticleNum()
+{
+ return m_ParticleNum;
}
template <class ParticleType>
-double ParticleFilter<ParticleType>::random01(unsigned long init) const {
- static unsigned long n;
- if (init > 0) {
- n = init;
- }
- n = 1664525 * n + 1013904223;
- // create double from unsigned long
- return (double)(n / 2) / (double)LONG_MAX;
+double ParticleFilter<ParticleType>::random01(unsigned long init) const
+{
+ static unsigned long n;
+ if (init > 0)
+ {
+ n = init;
+ }
+ n = 1664525 * n + 1013904223;
+ // create double from unsigned long
+ return (double)(n / 2) / (double)LONG_MAX;
}
template <class ParticleType>
-void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight) {
- // SOMETHING LEFT TO SORT?
- if (indexLeft >= indexRight) {
- // ready!
- return;
+void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight)
+{
+ // 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++;
}
-
- // 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
- ParticleType* temp = m_CurrentList[le];
- m_CurrentList[le] = m_CurrentList[ri];
- m_CurrentList[ri] = temp;
- le++;
- }
+ // skip from right
+ while ((ri >= first) && (m_CurrentList[ri]->getWeight() <=
+ m_CurrentList[pivot]->getWeight()))
+ {
+ ri--;
}
-
- if (le != pivot) {
- // swap
- ParticleType* temp = m_CurrentList[le];
- m_CurrentList[le] = m_CurrentList[pivot];
- m_CurrentList[pivot] = temp;
+ // now we have two elements to swap
+ if (le < ri)
+ {
+ // swap
+ ParticleType* temp = m_CurrentList[le];
+ m_CurrentList[le] = m_CurrentList[ri];
+ m_CurrentList[ri] = temp;
+ le++;
}
-
- // sort left side
- sort(indexLeft, le - 1);
- // sort right side
- sort(le + 1, indexRight);
+ }
+
+ if (le != pivot)
+ {
+ // swap
+ 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);
}
template <class ParticleType>
-void ParticleFilter<ParticleType>::normalize() {
- float weightSum = 0.0;
- for (int i = 0; i < m_ParticleNum; i++) {
- weightSum += m_CurrentList[i]->getWeight();
- }
- // only normalize if weightSum is big enough to divide
- if (weightSum > 0.000001) {
- // calculating parallel on 8 threats
- omp_set_num_threads(8);
- int i = 0;
- // #pragma omp parallel for
- for (i = 0; i < m_ParticleNum; i++) {
- float newWeight = m_CurrentList[i]->getWeight() / weightSum;
- m_CurrentList[i]->setWeight(newWeight);
- }
- } else {
- ROS_WARN_STREAM("Particle weights VERY small: " << weightSum << ". Got "
- << m_ParticleNum
- << " particles.");
+void ParticleFilter<ParticleType>::normalize()
+{
+ float weightSum = 0.0;
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ weightSum += m_CurrentList[i]->getWeight();
+ }
+ // only normalize if weightSum is big enough to divide
+ if (weightSum > 0.000001)
+ {
+ // calculating parallel on 8 threats
+ omp_set_num_threads(8);
+ int i = 0;
+ // #pragma omp parallel for
+ for (i = 0; i < m_ParticleNum; i++)
+ {
+ float newWeight = m_CurrentList[i]->getWeight() / weightSum;
+ m_CurrentList[i]->setWeight(newWeight);
}
+ }
+ else
+ {
+ ROS_WARN_STREAM("Particle weights VERY small: "
+ << weightSum << ". Got " << m_ParticleNum << " particles.");
+ }
}
template <class ParticleType>
-void ParticleFilter<ParticleType>::resample() {
- // swap pointers
- ParticleType** help = m_LastList;
- m_LastList = m_CurrentList;
- m_CurrentList = help;
- // now we copy from m_LastList to m_CurrentList
-
- int drawIndex = 0;
- // index of the particle where we are drawing to
- int targetIndex = 0;
-
- int numToDraw = 0;
- do {
- numToDraw = static_cast<int>(
- round((m_ParticleNum * m_LastList[drawIndex]->getWeight()) + 0.5));
- for (int i = 0; i < numToDraw; i++) {
- *m_CurrentList[targetIndex++] = *m_LastList[drawIndex];
- // don't draw too much
- if (targetIndex >= m_ParticleNum) {
- break;
- }
- }
- drawIndex++;
- } while (numToDraw > 0 && targetIndex < m_ParticleNum);
-
- // fill the rest of the particle list
- for (int i = targetIndex; i < m_ParticleNum; i++) {
- float particlePos = random01();
- float weightSum = 0.0;
- drawIndex = 0;
- weightSum += m_LastList[drawIndex]->getWeight();
- while (weightSum < particlePos) {
- weightSum += m_LastList[++drawIndex]->getWeight();
- }
- *m_CurrentList[i] = *m_LastList[drawIndex];
+void ParticleFilter<ParticleType>::resample()
+{
+ // swap pointers
+ ParticleType** help = m_LastList;
+ m_LastList = m_CurrentList;
+ m_CurrentList = help;
+ // now we copy from m_LastList to m_CurrentList
+
+ int drawIndex = 0;
+ // index of the particle where we are drawing to
+ int targetIndex = 0;
+
+ int numToDraw = 0;
+ do
+ {
+ numToDraw = static_cast<int>(
+ round((m_ParticleNum * m_LastList[drawIndex]->getWeight()) + 0.5));
+ for (int i = 0; i < numToDraw; i++)
+ {
+ *m_CurrentList[targetIndex++] = *m_LastList[drawIndex];
+ // don't draw too much
+ if (targetIndex >= m_ParticleNum)
+ {
+ break;
+ }
}
+ drawIndex++;
+ } while (numToDraw > 0 && targetIndex < m_ParticleNum);
+
+ // fill the rest of the particle list
+ for (int i = targetIndex; i < m_ParticleNum; i++)
+ {
+ float particlePos = random01();
+ float weightSum = 0.0;
+ drawIndex = 0;
+ weightSum += m_LastList[drawIndex]->getWeight();
+ while (weightSum < particlePos)
+ {
+ weightSum += m_LastList[++drawIndex]->getWeight();
+ }
+ *m_CurrentList[i] = *m_LastList[drawIndex];
+ }
}
template <class ParticleType>
-int ParticleFilter<ParticleType>::getEffectiveParticleNum() const {
- // does not work with normalized particle weights
- // does not work with our weights at all (algorithm of Grisetti)
- float squareSum = 0;
- for (int i = 0; i < m_ParticleNum; i++) {
- float weight = m_CurrentList[i]->getWeight();
- squareSum += weight * weight;
- }
- return static_cast<int>(1.0f / squareSum);
+int ParticleFilter<ParticleType>::getEffectiveParticleNum() const
+{
+ // does not work with normalized particle weights
+ // does not work with our weights at all (algorithm of Grisetti)
+ float squareSum = 0;
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ float weight = m_CurrentList[i]->getWeight();
+ squareSum += weight * weight;
+ }
+ return static_cast<int>(1.0f / squareSum);
}
template <class ParticleType>
-int ParticleFilter<ParticleType>::getEffectiveParticleNum2() const {
- // does not work with normalized particle weights
- int effectiveParticleNum = 0;
- for (int i = 0; i < m_ParticleNum; i++) {
- if (m_CurrentList[i]->getWeight() > MIN_EFFECTIVE_PARTICLE_WEIGHT) {
- effectiveParticleNum++;
- }
+int ParticleFilter<ParticleType>::getEffectiveParticleNum2() const
+{
+ // does not work with normalized particle weights
+ int effectiveParticleNum = 0;
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ if (m_CurrentList[i]->getWeight() > MIN_EFFECTIVE_PARTICLE_WEIGHT)
+ {
+ effectiveParticleNum++;
}
- return effectiveParticleNum;
+ }
+ return effectiveParticleNum;
}
template <class ParticleType>
-ParticleType* ParticleFilter<ParticleType>::getBestParticle() const {
- return m_CurrentList[0];
+ParticleType* ParticleFilter<ParticleType>::getBestParticle() const
+{
+ return m_CurrentList[0];
}
#endif
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
index 2f2d2939ef6b82af14c938a7851c712e48f8d396..7f6dd0fc16f4b5bdb870cf7089d9c9c4556b312c 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
@@ -40,266 +40,266 @@ class OccupancyMap;
* @see ParticleFilter
* @see OccupancyMap
*/
-class SlamFilter : public ParticleFilter<SlamParticle> {
- public:
- /**
- * This constructor initializes the random number generator and sets the
- * member variables to the given values.
- * @param particleNum Number of particles to use.
- */
- SlamFilter(int particleNum);
-
- /// @brief copy constructor
- SlamFilter(SlamFilter& slamFilter);
-
- /**
- * The destructor releases the OccupancyMap and the particles.
- */
- ~SlamFilter();
-
- /**
- * 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);
-
- /**
- * @return The Pose of the most important particle (particle with highest
- * weight).
- */
- Pose getLikeliestPose();
-
- /**
- * This method can be used to retrieve the most likely map that is stored by
- * the particle filter.
- * @return Pointer to the most likely occupancy map.
- */
- OccupancyMap* getLikeliestMap() const;
-
- void resetHigh();
-
- /**
- * 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 (is being
- * copied)
- */
- 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);
-
- /**
- * @return Vector of current particle poses. The vector is sorted according
- * to the weights of the
- * particles. The pose of the particle with the highest value is the first
- * element of the vector.
- */
- std::vector<Pose> getParticlePoses() const;
-
- /**
- * @return vector of all particles
+class SlamFilter : public ParticleFilter<SlamParticle>
+{
+public:
+ /**
+ * This constructor initializes the random number generator and sets the
+ * member variables to the given values.
+ * @param particleNum Number of particles to use.
*/
- std::vector<SlamParticle*>* getParticles() const;
-
- /**
- * @return Vector of current particle weights. The vector is sorted by
- * weight, highest weight first.
- */
- std::vector<float> getParticleWeights() const;
-
- /**
- * Calculates and returns the variance of the current likeliest particle
- * poses.
- * The orientation of the particle is neglected.
- * @param The number of treated particles.
- * @param[out] poseVarianceX The variance of particle poses in x direction.
- * @param[out] poseVarianceY The variance of particle poses in y direction.
- * @param[out] poseVarianceT The variance of particle poses in T rotation.
- */
- void getPoseVariances(int particleNum, float& poseVarianceX,
- float& poseVarianceY, float& poseVarianceT);
-
- /**
- * This method reduces the number of particles used in this SlamFilter to
- * the given value.
- * @param newParticleNumber The new number of particles
- */
- void reduceParticleNumber(int newParticleNumber);
-
- /**
- * This method returns the contrast of the occupancy grid
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the
- * map
- */
- double evaluateByContrast();
-
- /**
- * This method passes a masking map to to the underlying occupancy map
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
-
- private:
- /**
- * This method filter outliers in the given laser scan
- * @param rawData the laser scan to check
- * @param maxDiff maximal difference between two adjacent ranges
- * @return filtered scan without outliers
- */
- vector<float> filterOutliers(sensor_msgs::LaserScanConstPtr rawData,
- float maxDiff);
-
- /**
- * This method generates Gauss-distributed random variables with the given
- * variance. The computation
- * method is the Polar Method that is described in the book "A First Course
- * on Probability" by Sheldon Ross.
- * @param variance The variance for the Gauss-distribution that is used to
- * generate the random variable.
- * @return A random variable that is N(0, variance) distributed.
- */
- double randomGauss(float variance = 1.0) const;
-
- /**
- * This method drifts the particles according to the last two odometry
- * readings (time t-1 and time t).
- */
- void drift(Transformation2D odoTrans);
-
- /**
- * This method weightens each particle according to the given laser
- * measurement in m_LaserData.
- */
- void measure(sensor_msgs::LaserScanPtr laserData);
-
- /**
- * For weightening the particles, the filter needs a map.
- * This variable holds a pointer to a map.
- * @see OccupancyMap
- */
- OccupancyMap* m_OccupancyMap;
-
-
- /**
- * This variable holds the rotation error that the robot makes while it is
- * rotating.
- * Has to be given in percent. Example: robot makes errors of 3 degrees
- * while making a 60 degrees
- * move -> error is 5% -> rotationErrorRotating = 5)
- */
- float m_Alpha1;
-
- /**
- * This variable holds the rotation error that the robot makes while it is
- * translating
- * (moving forward or backwards). Has to be given in degrees per meter.
- */
- float m_Alpha2;
-
- /**
- * This variable holds the translation error that the robot makes while it
- * is translating.
- * Has to be given in percent.
- */
- float m_Alpha3;
-
- /**
- * This variable holds the translation error that the robot makes while it
- * is rotating.
- * This error only carries weight, if a translation es performed at the same
- * time.
- * See also m_Alpha5.
- * Has to be given in milimeters per degree. Example: Robot makes a turn of
- * 10 degrees and moves its
- * center unintentional 15 mm. -> translationErrorRotating = 15.0 / 10.0 =
- * 1.5
- */
- float m_Alpha4;
-
- /**
- * This variable holds a move jitter that is considered if the robot is
- * turning.
- * Has to be given in milimeters per degree.
- */
- float m_Alpha5;
-
- /**
- * True if it is the first run of SlamFilter, false otherwise.
- */
- bool m_FirstRun;
-
- /**
- * This variabe is true, if the SlamFilter is used for mapping and updates
- * the map,
- * false if it is just used for self-localization.
- */
- bool m_DoMapping;
-
- /**
- * Time stamp of the last particle filter step
- */
-
- ros::Time m_LastMoveTime;
-
- /**
- * Calculates the square of given input f
- * @param f input
- * @return square of input
- */
- template <class T>
- T sqr(T f) {
- return f * f;
- }
+ SlamFilter(int particleNum);
+
+ /// @brief copy constructor
+ SlamFilter(SlamFilter& slamFilter);
+
+ /**
+ * The destructor releases the OccupancyMap and the particles.
+ */
+ ~SlamFilter();
+
+ /**
+ * 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);
+
+ /**
+ * @return The Pose of the most important particle (particle with highest
+ * weight).
+ */
+ Pose getLikeliestPose();
+
+ /**
+ * This method can be used to retrieve the most likely map that is stored by
+ * the particle filter.
+ * @return Pointer to the most likely occupancy map.
+ */
+ OccupancyMap* getLikeliestMap() const;
+
+ void resetHigh();
+
+ /**
+ * 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 (is being
+ * copied)
+ */
+ 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);
+
+ /**
+ * @return Vector of current particle poses. The vector is sorted according
+ * to the weights of the
+ * particles. The pose of the particle with the highest value is the first
+ * element of the vector.
+ */
+ std::vector<Pose> getParticlePoses() const;
+
+ /**
+ * @return vector of all particles
+ */
+ std::vector<SlamParticle*>* getParticles() const;
+
+ /**
+ * @return Vector of current particle weights. The vector is sorted by
+ * weight, highest weight first.
+ */
+ std::vector<float> getParticleWeights() const;
+
+ /**
+ * Calculates and returns the variance of the current likeliest particle
+ * poses.
+ * The orientation of the particle is neglected.
+ * @param The number of treated particles.
+ * @param[out] poseVarianceX The variance of particle poses in x direction.
+ * @param[out] poseVarianceY The variance of particle poses in y direction.
+ * @param[out] poseVarianceT The variance of particle poses in T rotation.
+ */
+ void getPoseVariances(int particleNum, float& poseVarianceX,
+ float& poseVarianceY, float& poseVarianceT);
+
+ /**
+ * This method reduces the number of particles used in this SlamFilter to
+ * the given value.
+ * @param newParticleNumber The new number of particles
+ */
+ void reduceParticleNumber(int newParticleNumber);
+
+ /**
+ * This method returns the contrast of the occupancy grid
+ * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the
+ * map
+ */
+ double evaluateByContrast();
+
+ /**
+ * This method passes a masking map to to the underlying occupancy map
+ */
+ void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+
+private:
+ /**
+ * This method filter outliers in the given laser scan
+ * @param rawData the laser scan to check
+ * @param maxDiff maximal difference between two adjacent ranges
+ * @return filtered scan without outliers
+ */
+ vector<float> filterOutliers(sensor_msgs::LaserScanConstPtr rawData,
+ float maxDiff);
+
+ /**
+ * This method generates Gauss-distributed random variables with the given
+ * variance. The computation
+ * method is the Polar Method that is described in the book "A First Course
+ * on Probability" by Sheldon Ross.
+ * @param variance The variance for the Gauss-distribution that is used to
+ * generate the random variable.
+ * @return A random variable that is N(0, variance) distributed.
+ */
+ double randomGauss(float variance = 1.0) const;
+
+ /**
+ * This method drifts the particles according to the last two odometry
+ * readings (time t-1 and time t).
+ */
+ void drift(Transformation2D odoTrans);
+
+ /**
+ * This method weightens each particle according to the given laser
+ * measurement in m_LaserData.
+ */
+ void measure(sensor_msgs::LaserScanPtr laserData);
+
+ /**
+ * For weightening the particles, the filter needs a map.
+ * This variable holds a pointer to a map.
+ * @see OccupancyMap
+ */
+ OccupancyMap* m_OccupancyMap;
+
+ /**
+ * This variable holds the rotation error that the robot makes while it is
+ * rotating.
+ * Has to be given in percent. Example: robot makes errors of 3 degrees
+ * while making a 60 degrees
+ * move -> error is 5% -> rotationErrorRotating = 5)
+ */
+ float m_Alpha1;
+
+ /**
+ * This variable holds the rotation error that the robot makes while it is
+ * translating
+ * (moving forward or backwards). Has to be given in degrees per meter.
+ */
+ float m_Alpha2;
+
+ /**
+ * This variable holds the translation error that the robot makes while it
+ * is translating.
+ * Has to be given in percent.
+ */
+ float m_Alpha3;
+
+ /**
+ * This variable holds the translation error that the robot makes while it
+ * is rotating.
+ * This error only carries weight, if a translation es performed at the same
+ * time.
+ * See also m_Alpha5.
+ * Has to be given in milimeters per degree. Example: Robot makes a turn of
+ * 10 degrees and moves its
+ * center unintentional 15 mm. -> translationErrorRotating = 15.0 / 10.0 =
+ * 1.5
+ */
+ float m_Alpha4;
+
+ /**
+ * This variable holds a move jitter that is considered if the robot is
+ * turning.
+ * Has to be given in milimeters per degree.
+ */
+ float m_Alpha5;
+
+ /**
+ * True if it is the first run of SlamFilter, false otherwise.
+ */
+ bool m_FirstRun;
+
+ /**
+ * This variabe is true, if the SlamFilter is used for mapping and updates
+ * the map,
+ * false if it is just used for self-localization.
+ */
+ bool m_DoMapping;
+
+ /**
+ * Time stamp of the last particle filter step
+ */
+
+ ros::Time m_LastMoveTime;
+
+ /**
+ * Calculates the square of given input f
+ * @param f input
+ * @return square of input
+ */
+ template <class T>
+ T sqr(T f)
+ {
+ return f * f;
+ }
};
#endif
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
index 8cc3c11c9e275ce8d23c27d5debf11dbc6ec1872..63d50746bb39f7619fd69b2ecf23944a364f56d7 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
@@ -1,8 +1,8 @@
#ifndef SLAMPARTICLE_H
#define SLAMPARTICLE_H
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <homer_mapping/ParticleFilter/Particle.h>
#include <homer_nav_libs/Math/Pose.h>
@@ -14,7 +14,8 @@
*
* @brief This class defines a particle for the SlamFilter.
*
- * This particle contains a weight (inherited from base class) and a Pose (position + orientation).
+ * This particle contains a weight (inherited from base class) and a Pose
+ * (position + orientation).
* The Pose describes a possible position and orientation of the robot.
*
* @see SlamFilter
@@ -22,54 +23,52 @@
*/
class SlamParticle : public Particle
{
+public:
+ /**
+ * This constructor assigns the given weight to the member m_Weight.
+ * @param weight The weight of the particle.
+ * @param robotX X-Position of the robot (world coordinates in m).
+ * @param robotY Y-Position of the robot (world coordinates in m).
+ * @param robotTheta Orientation of the robot (radiants).
+ */
+ SlamParticle(float weight = 1.0, float robotX = 0.0, float robotY = 0.0,
+ float robotTheta = 0.0);
- public:
- /**
- * This constructor assigns the given weight to the member m_Weight.
- * @param weight The weight of the particle.
- * @param robotX X-Position of the robot (world coordinates in m).
- * @param robotY Y-Position of the robot (world coordinates in m).
- * @param robotTheta Orientation of the robot (radiants).
- */
- SlamParticle ( float weight = 1.0, float robotX = 0.0, float robotY = 0.0, float robotTheta = 0.0 );
-
- ///@brief copy contructor
- SlamParticle ( SlamParticle& slamParticle );
-
- /**
- * The destructor does nothing so far.
- */
- ~SlamParticle();
+ ///@brief copy contructor
+ SlamParticle(SlamParticle& slamParticle);
- /**
- * Sets the three members m_RobotPositionX, m_RobotPositionY, m_RobotOrientation.
- * @param robotX X-Position of the robot (world coordinates in m).
- * @param robotY Y-Position of the robot (world coordinates in m).
- * @param robotTheta Orientation of the robot (radiants).
- */
- void setRobotPose ( float robotX, float robotY, float robotTheta );
- void setRobotPose (Pose pose);
+ /**
+ * The destructor does nothing so far.
+ */
+ ~SlamParticle();
- /**
- * Returns the content of the three members m_RobotPositionX, m_RobotPositionY, m_RobotOrientation.
- * @param[out] robotX X-Position of the robot (world coordinates in m).
- * @param[out] robotY Y-Position of the robot (world coordinates in m).
- * @param[out] robotTheta Orientation of the robot (radiants).
- */
- void getRobotPose ( float& robotX, float& robotY, float& robotTheta );
- Pose getRobotPose ();
+ /**
+ * Sets the three members m_RobotPositionX, m_RobotPositionY,
+ * m_RobotOrientation.
+ * @param robotX X-Position of the robot (world coordinates in m).
+ * @param robotY Y-Position of the robot (world coordinates in m).
+ * @param robotTheta Orientation of the robot (radiants).
+ */
+ void setRobotPose(float robotX, float robotY, float robotTheta);
+ void setRobotPose(Pose pose);
+ /**
+ * Returns the content of the three members m_RobotPositionX,
+ * m_RobotPositionY, m_RobotOrientation.
+ * @param[out] robotX X-Position of the robot (world coordinates in m).
+ * @param[out] robotY Y-Position of the robot (world coordinates in m).
+ * @param[out] robotTheta Orientation of the robot (radiants).
+ */
+ void getRobotPose(float& robotX, float& robotY, float& robotTheta);
+ Pose getRobotPose();
- private:
-
- /**
- * These members store the pose of the robot.
- */
- float m_RobotPositionX;
- float m_RobotPositionY;
- float m_RobotOrientation;
-
+private:
+ /**
+ * These members store the pose of the robot.
+ */
+ float m_RobotPositionX;
+ float m_RobotPositionY;
+ float m_RobotOrientation;
};
#endif
-
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index 9be8905053d876f5bdfcbfa2ad950225e020a3e3..73cd1a94a44ad5220cff8d70c1b235b247e01ba8 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -203,8 +203,10 @@ void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
float lastValidRange = m_FreeReadingDistance;
RangeMeasurement rangeMeasurement;
- rangeMeasurement.sensorPos.x = getLaserTransform(laserData->header.frame_id).getOrigin().getX();
- rangeMeasurement.sensorPos.y = getLaserTransform(laserData->header.frame_id).getOrigin().getY();
+ rangeMeasurement.sensorPos.x =
+ getLaserTransform(laserData->header.frame_id).getOrigin().getX();
+ rangeMeasurement.sensorPos.y =
+ getLaserTransform(laserData->header.frame_id).getOrigin().getY();
for (unsigned int i = 0; i < laserData->ranges.size(); i++)
{
@@ -306,7 +308,7 @@ void OccupancyMap::insertRanges(vector<RangeMeasurement> ranges,
{
continue;
}
- if(ranges[i].range <= m_FreeReadingDistance)
+ if (ranges[i].range <= m_FreeReadingDistance)
{
continue;
}
@@ -477,28 +479,28 @@ double OccupancyMap::evaluateByContrast()
tf::StampedTransform OccupancyMap::getLaserTransform(std::string frame_id)
{
-if(m_savedTransforms.find(frame_id) != m_savedTransforms.end())
-{
- return m_savedTransforms[frame_id];
-}
-else
-{
- try
+ if (m_savedTransforms.find(frame_id) != m_savedTransforms.end())
{
- m_tfListener.waitForTransform("/base_link", frame_id,
- ros::Time(0), ros::Duration(0.2));
- m_tfListener.lookupTransform("/base_link", frame_id,
- ros::Time(0), m_savedTransforms[frame_id]);
return m_savedTransforms[frame_id];
}
- catch (tf::TransformException ex)
+ else
{
- ROS_ERROR_STREAM(ex.what());
- ROS_ERROR_STREAM("need transformation from base_link to laser!");
+ try
+ {
+ m_tfListener.waitForTransform("/base_link", frame_id, ros::Time(0),
+ ros::Duration(0.2));
+ m_tfListener.lookupTransform("/base_link", frame_id, ros::Time(0),
+ m_savedTransforms[frame_id]);
+ return m_savedTransforms[frame_id];
+ }
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR_STREAM(ex.what());
+ ROS_ERROR_STREAM("need transformation from base_link to laser!");
+ }
}
-}
-return tf::StampedTransform();
+ return tf::StampedTransform();
}
vector<MeasurePoint>
@@ -708,7 +710,7 @@ void OccupancyMap::drawLine(Eigen::Vector2i& startPixel,
{
continue;
}
- if (m_MapPoints[index].CurrentChange == ::NO_CHANGE||
+ if (m_MapPoints[index].CurrentChange == ::NO_CHANGE ||
m_MapPoints[index].CurrentChange == ::FREE)
{
m_MapPoints[index].CurrentChange = value;
diff --git a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp b/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
index e2b1b45893be9772d60fb339496c8a15ba79d040..8fa1f76ce070e081c7e8a62acd05dd04065e219f 100644
--- a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
@@ -1,36 +1,36 @@
#include <homer_mapping/ParticleFilter/HyperSlamFilter.h>
-#include <vector>
+#include <stdlib.h>
#include <cmath>
#include <fstream>
#include <sstream>
-#include <stdlib.h>
+#include <vector>
#include "ros/ros.h"
using namespace std;
-HyperSlamFilter::HyperSlamFilter (int particleFilterNum, int particleNum )
+HyperSlamFilter::HyperSlamFilter(int particleFilterNum, int particleNum)
{
m_ParticleFilterNum = particleFilterNum;
- if ( m_ParticleFilterNum < 1 )
- {
- m_ParticleFilterNum = 1;
- }
- ROS_DEBUG( "Using %d Hyper Particles.", particleFilterNum);
+ if (m_ParticleFilterNum < 1)
+ {
+ m_ParticleFilterNum = 1;
+ }
+ ROS_DEBUG("Using %d Hyper Particles.", particleFilterNum);
m_ParticleNum = particleNum;
- m_DoMapping = true;
+ m_DoMapping = true;
m_DeletionThreshold = 0.98;
- for ( unsigned i=0; i < m_ParticleFilterNum; i++ )
+ for (unsigned i = 0; i < m_ParticleFilterNum; i++)
{
ostringstream stream;
stream << "SlamFilter " << i;
- SlamFilter *slamFilter = new SlamFilter ( particleNum );
- m_SlamFilters.push_back ( slamFilter );
+ SlamFilter* slamFilter = new SlamFilter(particleNum);
+ m_SlamFilters.push_back(slamFilter);
}
m_BestSlamFilter = m_SlamFilters[0];
@@ -40,7 +40,7 @@ HyperSlamFilter::~HyperSlamFilter()
{
for (unsigned i = 0; i < m_ParticleFilterNum; i++)
{
- if( m_SlamFilters[i] )
+ if (m_SlamFilters[i])
{
delete m_SlamFilters[i];
m_SlamFilters[i] = 0;
@@ -48,139 +48,144 @@ HyperSlamFilter::~HyperSlamFilter()
}
}
-void HyperSlamFilter::setRotationErrorRotating ( float percent )
+void HyperSlamFilter::setRotationErrorRotating(float percent)
{
- for ( unsigned i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setRotationErrorRotating(percent / 100.0);
- }
+ for (unsigned i = 0; i < m_SlamFilters.size(); i++)
+ {
+ m_SlamFilters[i]->setRotationErrorRotating(percent / 100.0);
+ }
}
-void HyperSlamFilter::setRotationErrorTranslating ( float degreePerMeter )
+void HyperSlamFilter::setRotationErrorTranslating(float degreePerMeter)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setRotationErrorTranslating(degreePerMeter / 180.0 * M_PI);
+ m_SlamFilters[i]->setRotationErrorTranslating(degreePerMeter / 180.0 *
+ M_PI);
}
}
-void HyperSlamFilter::setTranslationErrorTranslating ( float percent )
+void HyperSlamFilter::setTranslationErrorTranslating(float percent)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
m_SlamFilters[i]->setTranslationErrorTranslating(percent / 100.0);
}
}
-void HyperSlamFilter::setTranslationErrorRotating ( float mPerDegree )
+void HyperSlamFilter::setTranslationErrorRotating(float mPerDegree)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setTranslationErrorRotating( mPerDegree / 180.0 * M_PI );
+ m_SlamFilters[i]->setTranslationErrorRotating(mPerDegree / 180.0 * M_PI);
}
}
-void HyperSlamFilter::setMoveJitterWhileTurning ( float mPerDegree )
+void HyperSlamFilter::setMoveJitterWhileTurning(float mPerDegree)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setMoveJitterWhileTurning( mPerDegree / 180.0 * M_PI );
+ m_SlamFilters[i]->setMoveJitterWhileTurning(mPerDegree / 180.0 * M_PI);
}
}
void HyperSlamFilter::resetHigh()
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
m_SlamFilters[i]->resetHigh();
}
}
-void HyperSlamFilter::setMapping ( bool doMapping )
+void HyperSlamFilter::setMapping(bool doMapping)
{
- m_DoMapping = doMapping;
+ m_DoMapping = doMapping;
}
-void HyperSlamFilter:: setOccupancyMap ( OccupancyMap* occupancyMap )
+void HyperSlamFilter::setOccupancyMap(OccupancyMap* occupancyMap)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setOccupancyMap( occupancyMap );
+ m_SlamFilters[i]->setOccupancyMap(occupancyMap);
}
}
-void HyperSlamFilter::setRobotPose ( Pose pose, double scatterVarXY, double scatterVarTheta )
+void HyperSlamFilter::setRobotPose(Pose pose, double scatterVarXY,
+ double scatterVarTheta)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
m_SlamFilters[i]->setRobotPose(pose, scatterVarXY, scatterVarTheta);
}
}
-void HyperSlamFilter::filter ( Transformation2D trans, sensor_msgs::LaserScanConstPtr laserData)
+void HyperSlamFilter::filter(Transformation2D trans,
+ sensor_msgs::LaserScanConstPtr laserData)
{
- //call filter methods of all particle filters
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ // call filter methods of all particle filters
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
bool randOnOff;
- if(m_SlamFilters.size() == 1)
- {
- randOnOff = true;
- }
- else
- {
- //if mapping is on, switch on with 80% probability to introduce some randomness in different particle filters
- randOnOff = (rand() % 100) < 80;
- }
- m_SlamFilters[i]->setMapping( m_DoMapping && randOnOff );
+ if (m_SlamFilters.size() == 1)
+ {
+ randOnOff = true;
+ }
+ else
+ {
+ // if mapping is on, switch on with 80% probability to introduce some
+ // randomness in different particle filters
+ randOnOff = (rand() % 100) < 80;
+ }
+ m_SlamFilters[i]->setMapping(m_DoMapping && randOnOff);
m_SlamFilters[i]->filter(trans, laserData);
-
}
- if(m_SlamFilters.size() != 1)
+ if (m_SlamFilters.size() != 1)
{
- //determine which map has the best/worst contrast
- double bestContrast = 0.0;
- static unsigned int bestFilter = 0;
- double worstContrast = 100.0;
- static unsigned int worstFilter = 0;
-
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- double contrast = m_SlamFilters[i]->evaluateByContrast();
- {
- if( contrast > bestContrast )
- {
- bestContrast = contrast;
- bestFilter = i;
- }
- if ( contrast < worstContrast )
- {
- worstContrast = contrast;
- worstFilter = i;
- }
- }
- }
-
- // set best filter
- SlamFilter* lastBestFilter = m_BestSlamFilter;
- m_BestSlamFilter = m_SlamFilters[bestFilter];
-
- if ( m_BestSlamFilter != lastBestFilter )
- {
- ROS_INFO( "Switched to best filter %d (bestContrast: %f) -- the worst filter is %d (worstContrast: %f)", bestFilter, bestContrast, worstFilter, worstContrast); //TODO
- }
-
- if ( bestFilter != worstFilter )
- {
- if ( worstContrast < ( bestContrast * m_DeletionThreshold ) )
- {
- // replace the worst filter by the one with the best contrast
- delete m_SlamFilters[worstFilter];
- m_SlamFilters[worstFilter] = new SlamFilter ( * m_SlamFilters [bestFilter] );
- }
- }
- }
+ // determine which map has the best/worst contrast
+ double bestContrast = 0.0;
+ static unsigned int bestFilter = 0;
+ double worstContrast = 100.0;
+ static unsigned int worstFilter = 0;
+
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
+ {
+ double contrast = m_SlamFilters[i]->evaluateByContrast();
+ {
+ if (contrast > bestContrast)
+ {
+ bestContrast = contrast;
+ bestFilter = i;
+ }
+ if (contrast < worstContrast)
+ {
+ worstContrast = contrast;
+ worstFilter = i;
+ }
+ }
+ }
+
+ // set best filter
+ SlamFilter* lastBestFilter = m_BestSlamFilter;
+ m_BestSlamFilter = m_SlamFilters[bestFilter];
+
+ if (m_BestSlamFilter != lastBestFilter)
+ {
+ ROS_INFO("Switched to best filter %d (bestContrast: %f) -- the worst "
+ "filter is %d (worstContrast: %f)",
+ bestFilter, bestContrast, worstFilter, worstContrast); // TODO
+ }
+
+ if (bestFilter != worstFilter)
+ {
+ if (worstContrast < (bestContrast * m_DeletionThreshold))
+ {
+ // replace the worst filter by the one with the best contrast
+ delete m_SlamFilters[worstFilter];
+ m_SlamFilters[worstFilter] = new SlamFilter(*m_SlamFilters[bestFilter]);
+ }
+ }
+ }
}
SlamFilter* HyperSlamFilter::getBestSlamFilter()
diff --git a/homer_mapping/src/ParticleFilter/SlamParticle.cpp b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
index 3d564cde000959b43ecf3bf6b53454a604ac95a4..d88fa0dfb8dd50ffcac377f3d908e26bb68601b7 100644
--- a/homer_mapping/src/ParticleFilter/SlamParticle.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
@@ -1,41 +1,47 @@
#include <homer_mapping/ParticleFilter/SlamParticle.h>
-SlamParticle::SlamParticle(float weight, float robotX, float robotY, float robotTheta) : Particle(weight) {
+SlamParticle::SlamParticle(float weight, float robotX, float robotY,
+ float robotTheta)
+ : Particle(weight)
+{
m_RobotPositionX = robotX;
m_RobotPositionY = robotY;
m_RobotOrientation = robotTheta;
}
-SlamParticle::SlamParticle( SlamParticle& slamParticle )
+SlamParticle::SlamParticle(SlamParticle& slamParticle)
{
m_RobotPositionX = slamParticle.m_RobotPositionX;
m_RobotPositionY = slamParticle.m_RobotPositionY;
m_RobotOrientation = slamParticle.m_RobotOrientation;
}
-SlamParticle::~SlamParticle() {
+SlamParticle::~SlamParticle()
+{
}
-void SlamParticle::setRobotPose(float robotX, float robotY, float robotTheta) {
+void SlamParticle::setRobotPose(float robotX, float robotY, float robotTheta)
+{
m_RobotPositionX = robotX;
m_RobotPositionY = robotY;
m_RobotOrientation = robotTheta;
}
-void SlamParticle::setRobotPose(Pose pose){
- m_RobotPositionX = pose.x();
- m_RobotPositionY = pose.y();
- m_RobotOrientation = pose.theta();
+void SlamParticle::setRobotPose(Pose pose)
+{
+ m_RobotPositionX = pose.x();
+ m_RobotPositionY = pose.y();
+ m_RobotOrientation = pose.theta();
}
-void SlamParticle::getRobotPose(float& robotX, float& robotY, float& robotTheta) {
+void SlamParticle::getRobotPose(float& robotX, float& robotY, float& robotTheta)
+{
robotX = m_RobotPositionX;
robotY = m_RobotPositionY;
robotTheta = m_RobotOrientation;
}
-Pose SlamParticle::getRobotPose(){
- return Pose(m_RobotPositionX, m_RobotPositionY, m_RobotOrientation);
+Pose SlamParticle::getRobotPose()
+{
+ return Pose(m_RobotPositionX, m_RobotPositionY, m_RobotOrientation);
}
-
-