diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index 9660e7ce1a441ad9f9bd71bd404c9d09d2588101..8d3c2e42fa90c0b235000be56da3fe59cf7cb9f7 100644
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
@@ -318,8 +318,8 @@ protected:
* @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);
+ void drawLine(Eigen::Vector2i& startPixel, Eigen::Vector2i& endPixel,
+ char value);
/**
* This method computes the values for m_OccupancyProbabilities from
@@ -361,7 +361,6 @@ protected:
*/
void resetExploredRegion();
-
/**
* Stores the metadata of the map
*/
@@ -394,7 +393,6 @@ protected:
// Used for high Sensitive areas
unsigned short* m_HighSensitive;
-
struct PixelValue
{
float OccupancyProbability;
@@ -405,11 +403,11 @@ protected:
PixelValue()
{
- OccupancyProbability = UNKNOWN_LIKELIHOOD;
- OccupancyCount = 0;
- MeasurementCount = 0;
- CurrentChange = NO_CHANGE;
- HighSensitive = 0;
+ OccupancyProbability = UNKNOWN_LIKELIHOOD;
+ OccupancyCount = 0;
+ MeasurementCount = 0;
+ CurrentChange = NO_CHANGE;
+ HighSensitive = 0;
}
};
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
old mode 100755
new mode 100644
index ac9f1553392b80cc9678e3341745628ac5a20b84..b377d95baf664b20b420be20ec1cb71528d30f1e
--- a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
@@ -1,12 +1,12 @@
#ifndef HYPERSLAMFILTER_H
#define HYPERSLAMFILTER_H
-#include <vector>
+#include <homer_mapping/OccupancyMap/OccupancyMap.h>
#include <homer_mapping/ParticleFilter/ParticleFilter.h>
-#include <homer_mapping/ParticleFilter/SlamParticle.h>
#include <homer_mapping/ParticleFilter/SlamFilter.h>
+#include <homer_mapping/ParticleFilter/SlamParticle.h>
#include <homer_nav_libs/Math/Pose.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
+#include <vector>
#include <sensor_msgs/LaserScan.h>
@@ -17,138 +17,148 @@ class OccupancyMap;
*
* @author Malte Knauf, Stephan Wirth, Susanne Maur
*
- * @brief This class is used to determine the robot's most likely pose with given map and given laser data.
+ * @brief This class is used to determine the robot's most likely pose with
+ * given map and given laser data.
*
- * A particle filter is a descrete method to describe and compute with a probability distribution.
- * This particle filter uses an occupancy map to determine the probability of robot states.
- * The robot states are stored in a particle together with their weight @see SlamParticle.
+ * A particle filter is a descrete method to describe and compute with a
+ * probability distribution.
+ * This particle filter uses an occupancy map to determine the probability of
+ * robot states.
+ * The robot states are stored in a particle together with their weight @see
+ * SlamParticle.
*
* @see SlamParticle
* @see ParticleFilter
* @see 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)
+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)
{
- for(unsigned i = 0; i < m_ParticleFilterNum; ++i)
- {
- m_SlamFilters[i]->applyMasking(msg);
- }
+ m_SlamFilters[i]->applyMasking(msg);
}
+ }
- private:
-
- /** Used SlamFilters */
- std::vector <SlamFilter*> m_SlamFilters;
+private:
+ /** 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;
-
- bool m_DoMapping;
+ /** Worst SlamFilter */
+ SlamFilter* m_WorstSlamFilter;
+ bool m_DoMapping;
};
#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
old mode 100755
new mode 100644
index e2b1b45893be9772d60fb339496c8a15ba79d040..8fa1f76ce070e081c7e8a62acd05dd04065e219f
--- 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()