Commit 851536ff authored by Daniel Müller's avatar Daniel Müller
Browse files

Added some const statements where applicable

parent fec17858
......@@ -197,7 +197,7 @@ public:
* @brief gives a list specially processed coordinates to be used for
* computeLaserScanProbability
*/
std::vector<MeasurePoint> getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData);
std::vector<MeasurePoint> getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData) const;
/**
* This method computes a score that describes how good the given hypothesis
......@@ -208,7 +208,7 @@ public:
* This factor is NOT normalized, it is a positive float between 0
* and FLOAT_MAX
*/
float computeScore(Pose robotPose, std::vector<MeasurePoint> measurePoints);
float computeScore(Pose robotPose, const std::vector<MeasurePoint>& measurePoints);
/**
* @return QImage of size m_metaData.width, m_metaData.height with values of
......@@ -268,7 +268,7 @@ public:
void changeMapSize(int x_add_left, int y_add_up, int x_add_right, int y_add_down);
tf::StampedTransform getLaserTransform(std::string frame_id);
tf::StampedTransform getLaserTransform(std::string frame_id) const;
protected:
/**
......
......@@ -462,11 +462,11 @@ double OccupancyMap::evaluateByContrast()
return (0);
}
tf::StampedTransform OccupancyMap::getLaserTransform(std::string frame_id)
tf::StampedTransform OccupancyMap::getLaserTransform(std::string frame_id) const
{
if (m_savedTransforms.find(frame_id) != m_savedTransforms.end())
{
return m_savedTransforms[frame_id];
return m_savedTransforms.at(frame_id);
}
else
{
......@@ -475,7 +475,7 @@ tf::StampedTransform OccupancyMap::getLaserTransform(std::string frame_id)
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];
return m_savedTransforms.at(frame_id);
}
catch (tf::TransformException ex)
{
......@@ -487,27 +487,28 @@ tf::StampedTransform OccupancyMap::getLaserTransform(std::string frame_id)
return tf::StampedTransform();
}
vector<MeasurePoint> OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData)
vector<MeasurePoint> OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData) const
{
vector<MeasurePoint> result;
result.reserve(laserData->ranges.size());
double minDist = m_MeasureSamplingStep;
const double minDist = m_MeasureSamplingStep;
Point2D lastHitPos;
Point2D lastUsedHitPos;
// extract points for measuring
for (unsigned int i = 0; i < laserData->ranges.size(); i++)
int laser_count = 0;
for (const auto& range : laserData->ranges)
{
if (laserData->ranges[i] <= laserData->range_max
&& laserData->ranges[i] >= laserData->range_min)
if (range <= laserData->range_max && range >= laserData->range_min)
{
float alpha = laserData->angle_min + i * laserData->angle_increment;
float alpha = laserData->angle_min
+ static_cast<float>(laser_count) * laserData->angle_increment;
tf::Vector3 pin;
tf::Vector3 pout;
pin.setX(cos(alpha) * laserData->ranges[i]);
pin.setY(sin(alpha) * laserData->ranges[i]);
pin.setX(cos(alpha) * range);
pin.setY(sin(alpha) * range);
pin.setZ(0);
pout = getLaserTransform(laserData->header.frame_id) * pin;
......@@ -516,9 +517,12 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstP
if (hitPos.distance(lastUsedHitPos) >= minDist)
{
MeasurePoint p;
// in-place construction in returned result
result.emplace_back();
auto& p = result.back();
// preserve borders of segments
if ((i != 0) && (lastUsedHitPos.distance(lastHitPos) > m_metaData.resolution * 0.5)
if ((laser_count != 0)
&& (lastUsedHitPos.distance(lastHitPos) > m_metaData.resolution * 0.5)
&& (hitPos.distance(lastHitPos) >= minDist * 1.5))
{
p.hitPos = lastHitPos;
......@@ -530,12 +534,13 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstP
{
p.borderType = NoBorder;
}
// TODO (DM): Given positive if statement, this attribute gets re-written!
p.hitPos = hitPos;
result.push_back(p);
lastUsedHitPos = hitPos;
}
lastHitPos = hitPos;
}
laser_count++;
}
// the first and last one are border pixels
......@@ -574,7 +579,7 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstP
return result;
}
float OccupancyMap::computeScore(Pose robotPose, std::vector<MeasurePoint> measurePoints)
float OccupancyMap::computeScore(Pose robotPose, const std::vector<MeasurePoint>& measurePoints)
{
// This is a very simple implementation, using only the end point of the
// beam.
......
......@@ -381,7 +381,7 @@ void SlamFilter::measure(sensor_msgs::LaserScanPtr laserData)
{
if (m_OccupancyMap)
{
std::vector<MeasurePoint> measurePoints = m_OccupancyMap->getMeasurePoints(laserData);
const auto& measurePoints = m_OccupancyMap->getMeasurePoints(laserData);
for (int i = 0; i < m_ParticleNum; i++)
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment