From d54b65523e808245357a45e6912b9957d12384c2 Mon Sep 17 00:00:00 2001 From: Timothy Wiley Date: Fri, 28 Apr 2017 13:54:23 +1000 Subject: [PATCH] crosbot_fastslam: more code cleanup --- .../include/crosbot_fastslam/fastslam.hpp | 6 +- .../include/crosbot_fastslam/heightmap.hpp | 154 +++++------------ .../include/crosbot_fastslam/particle.hpp | 22 +-- crosbot_fastslam/src/fastslam.cpp | 109 ++++++------ crosbot_fastslam/src/heightmap.cpp | 159 ++++++++++++++++-- crosbot_fastslam/src/particle.cpp | 20 +++ 6 files changed, 266 insertions(+), 204 deletions(-) diff --git a/crosbot_fastslam/include/crosbot_fastslam/fastslam.hpp b/crosbot_fastslam/include/crosbot_fastslam/fastslam.hpp index a41f5dd..1cfe277 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/fastslam.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/fastslam.hpp @@ -134,12 +134,14 @@ protected: */ FastSLAMParameters parameters; MotionModel motionModel; - double motionSumXY, motionSumTheta; + double motionSumXY; + double motionSumTheta; /** * The mean and motion particles */ - ParticlePtr mean, motion; + ParticlePtr mean; + ParticlePtr motion; /** * All of the particles diff --git a/crosbot_fastslam/include/crosbot_fastslam/heightmap.hpp b/crosbot_fastslam/include/crosbot_fastslam/heightmap.hpp index 537d537..2b3f3ce 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/heightmap.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/heightmap.hpp @@ -88,64 +88,37 @@ public: HeightVal* hlist; - HeightMap(unsigned int rows, unsigned int cols, float initialOdds) : - owner(NULL), rows(rows), columns(cols), hlist(NULL) - { - initMap(initialOdds); - } - - HeightMap(HeightMapPtr cpy) : - owner(NULL), rows(cpy->rows), columns(cpy->columns), - xOrig(cpy->xOrig), yOrig(cpy->yOrig), hlist(NULL) - { - initMap(); - if (hlist == NULL || cpy->hlist == NULL) { - ERROR("HeightMap: Attempted to copy empty height map.\n"); - } else { - memcpy(hlist, cpy->hlist, (rows * columns) * sizeof(HeightVal)); - } - } + HeightMap(unsigned int rows, unsigned int cols, float initialOdds); + HeightMap(HeightMapPtr cpy); + virtual ~HeightMap(); - ~HeightMap() { - if (hlist != NULL) { - free(hlist); - } - } - - inline void initMap() { - width = columns * CELL_WIDTH; - height = rows * CELL_HEIGHT; - hlist = (HeightVal *) malloc(sizeof(HeightVal) * rows * columns); - } + /** + * Initialise a new empty height map + */ + void initMap(); - inline void initMap(float initialOdds) { - width = columns * CELL_WIDTH; - height = rows * CELL_HEIGHT; - hlist = (HeightVal *) malloc(sizeof(HeightVal) * rows * columns); - zeroMap(initialOdds); - } + /** + * Initialise a new height map with an initial esimate of odds + */ + void initMap(float initialOdds); - inline void zeroMap(float initialOdds) { - unsigned int count = rows * columns; - for (unsigned int i = 0; i < count; i++) { - hlist[i].height = 0.0; - hlist[i].prob = initialOdds; - hlist[i].observations = 0; - hlist[i].searches = 0; - } - } + /** + * Zero the height map to a fixed odd + */ + void zeroMap(float initialOdds); /** * Returns a cell based upon its index */ inline HeightVal *getIndex(int idx) { - return &(hlist[idx]); + return &(hlist[idx]); } // returns a particular cell by row and column inline HeightVal *getByIJ(unsigned int i, unsigned int j) { - if ((i >= rows) || (j >= columns)) + if ((i >= rows) || (j >= columns)) { return NULL; + } return getIndex(i * columns + j); } @@ -166,8 +139,9 @@ public: // NOTE: this function isn't currently used since it's more efficient not to make invalid calls // This means it hasn't been tested in use inline bool checkWithinMap(float x, float y) { - if ((x < xOrig) || (x >= (xOrig + width)) || (y < yOrig) || (y >= (yOrig + height))) + if ((x < xOrig) || (x >= (xOrig + width)) || (y < yOrig) || (y >= (yOrig + height))) { return false; + } return true; } }; @@ -177,14 +151,7 @@ public: unsigned int rows, columns; std::vector< std::vector > array; - HeightMapArray(unsigned int rows, unsigned int columns) : - rows(rows), columns(columns) - { - array.resize(rows); - for (unsigned int r = 0; r < rows; r++) { - array[r].resize(columns); - } - } + HeightMapArray(unsigned int rows, unsigned int columns); inline std::vector& operator[](int row) { return array[row]; @@ -205,74 +172,33 @@ public: }; // x,y global position of the map - float offsetX, offsetY; -// unsigned int rows, columns, patchRows, patchColumns; -// float initialOdds; + float offsetX; + float offsetY; + FastSLAMParameters& parameters; - unsigned int rows, columns; + unsigned int rows; + unsigned int columns; HeightMapArray patches; - HeightMultiMap(FastSLAMParameters& parameters) : - parameters(parameters), - rows(parameters.mapRows), columns(parameters.mapColumns), - patches(rows, columns) - { - - zeroMap(); - offsetX = 0 - (columns * parameters.patchColumns * CELL_WIDTH / 2); - offsetY = 0 - (rows * parameters.patchRows * CELL_HEIGHT / 2); - } - - // performs shallow copy of all non-NULL HeightMaps - HeightMultiMap(HeightMultiMapPtr cpy) : - offsetX(cpy->offsetX), offsetY(cpy->offsetY), - parameters(cpy->parameters), - rows(parameters.mapRows), columns(parameters.mapColumns), - patches(rows, columns) - { - unsigned int i, j; + HeightMultiMap(FastSLAMParameters& parameters); - for (i = 0; i < rows; i++) { - for (j = 0; j < columns; j++) { - HeightMapPtr hm = cpy->patches[i][j]; - if (hm != NULL) { - patches[i][j] = hm; - hm->owner = NULL; - } - } - } - } + /** + * Performs shallow copy of all non-NULL HeightMaps + */ + HeightMultiMap(HeightMultiMapPtr cpy); - ~HeightMultiMap() { - releaseSubMaps(); - } + virtual ~HeightMultiMap(); - // set all submaps to NULL - void zeroMap() { - unsigned int i, j; - for (i = 0; i < rows; i++) { - for (j = 0; j < columns; j++) { - HeightMapPtr hm = patches[i][j]; - if (hm!= NULL && hm->owner == this) - hm->owner = NULL; - patches[i][j] = NULL; - } - } - } + /** + * Set all sub-maps to NULL + */ + void zeroMap(); - // release ownership of all submaps - void releaseSubMaps() { - unsigned int i, j; - for (i = 0; i < rows; i++) { - for (j = 0; j < columns; j++) { - HeightMapPtr hm = patches[i][j]; - if (hm != NULL && hm->owner == this) { - hm->owner = NULL; - } - } - } - } + /** + * Release ownership of all sub-maps + */ + void releaseSubMaps(); /** * raytraces to each point from the proper origin and updates the cell probabilities diff --git a/crosbot_fastslam/include/crosbot_fastslam/particle.hpp b/crosbot_fastslam/include/crosbot_fastslam/particle.hpp index a1fa2d2..9722a5e 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/particle.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/particle.hpp @@ -52,26 +52,12 @@ public: void addTag(TagPtr tag, bool checkForDuplicate = false); void applyMotion(tf::Transform motion, MapCloudPtr cloud, bool calculateWeight = true, double gain = DEFAULT_GAINVALUE); - void update(MapCloudPtr cloud); - MapCloudPtr getLatestUpdate() { return motionCloud; } - - inline PathPtr getPath() { - Lock lock(rwLock); - PathPtr rval = new Path(); - for (size_t i = 0; i < history.size(); i++) { - rval->path.push_back(history[i].pose); - if (history[i].cloud != NULL) - rval->timestamps.push_back(history[i].cloud->timestamp); - else - rval->timestamps.push_back(Time(0,0)); - } - - if (rval->path.size() == 0 || rval->path[rval->path.size()-1] != pose) - rval->path.push_back(pose); - - return rval; + void update(MapCloudPtr cloud); + MapCloudPtr getLatestUpdate() { + return motionCloud; } + PathPtr getPath(); void setPose(Pose pose) { Lock lock(rwLock, true); diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp index ac00e97..df655a1 100644 --- a/crosbot_fastslam/src/fastslam.cpp +++ b/crosbot_fastslam/src/fastslam.cpp @@ -572,66 +572,65 @@ void FastSLAMMap::updateParticles(MapCloudPtr cloud) { void FastSLAMMap::resample() { // TODO: combine this with resampleAndUpdate (as these two seem to be similar methods) - if (particles.size() == 0) { - return; - } - - ParticlePtr newMean = particles[0]; - double weightMax = newMean->weight; - double weightSum = 0; - for (size_t i = 0; i < particles.size(); i++) { - ParticlePtr p = particles[i]; - if (p->weight > weightMax) { - newMean = p; - weightMax = p->weight; - } - weightSum += p->weight; - } - - std::vector newParticles; - std::vector newPoses; - std::vector marks; - marks.resize(particles.size()); - memset(&marks[0], 0, marks.size()*sizeof(int)); + if (particles.size() != 0) { + + ParticlePtr newMean = particles[0]; + double weightMax = newMean->weight; + double weightSum = 0; + for (size_t i = 0; i < particles.size(); i++) { + ParticlePtr p = particles[i]; + if (p->weight > weightMax) { + newMean = p; + weightMax = p->weight; + } + weightSum += p->weight; + } - newParticles.resize(parameters.numberOfParticles); - newPoses.resize(newParticles.size()); - size_t i = 0; - if (parameters.addMotionCopy) { - newParticles[i] = new Particle(motion); - ++i; - } + std::vector newParticles; + std::vector newPoses; + std::vector marks; + marks.resize(particles.size()); + memset(&marks[0], 0, marks.size()*sizeof(int)); + + newParticles.resize(parameters.numberOfParticles); + newPoses.resize(newParticles.size()); + size_t i = 0; + if (parameters.addMotionCopy) { + newParticles[i] = new Particle(motion); + ++i; + } - for (; i < newParticles.size(); i++) { - double rand = ProbabilityTable.randxy(0, weightSum); - ParticlePtr p = particles[0]; - double wCurrent = p->weight; - size_t j = 0; - for (++j; j < particles.size() && wCurrent <= rand; j++) { - p = particles[j]; - wCurrent += p->weight; - } + for (; i < newParticles.size(); i++) { + double rand = ProbabilityTable.randxy(0, weightSum); + ParticlePtr p = particles[0]; + double wCurrent = p->weight; + size_t j = 0; + for (++j; j < particles.size() && wCurrent <= rand; j++) { + p = particles[j]; + wCurrent += p->weight; + } - j--; - if (marks[j]) { - newParticles[i] = new Particle(p); - newPoses[i] = p->pose; - } else { - newParticles[i] = p; - newPoses[i] = p->pose; - marks[j] = true; - } - } + j--; + if (marks[j]) { + newParticles[i] = new Particle(p); + newPoses[i] = p->pose; + } else { + newParticles[i] = p; + newPoses[i] = p->pose; + marks[j] = true; + } + } - { - Lock lock(particlesLock, true); + { + Lock lock(particlesLock, true); - mean = newMean; - particles.clear(); - particles = newParticles; - newParticles.clear(); - particlePoses = newPoses; - meanPose = mean->pose; + mean = newMean; + particles.clear(); + particles = newParticles; + newParticles.clear(); + particlePoses = newPoses; + meanPose = mean->pose; + } } } diff --git a/crosbot_fastslam/src/heightmap.cpp b/crosbot_fastslam/src/heightmap.cpp index ecd770b..fc568d5 100644 --- a/crosbot_fastslam/src/heightmap.cpp +++ b/crosbot_fastslam/src/heightmap.cpp @@ -13,6 +13,124 @@ namespace crosbot { namespace fastslam { +HeightMap::HeightMap(unsigned int rows, unsigned int cols, float initialOdds) : + owner(NULL), rows(rows), columns(cols), hlist(NULL) +{ + initMap(initialOdds); +} + +HeightMap::HeightMap(HeightMapPtr cpy) : + owner(NULL), rows(cpy->rows), columns(cpy->columns), + xOrig(cpy->xOrig), yOrig(cpy->yOrig), hlist(NULL) +{ + initMap(); + if (hlist == NULL || cpy->hlist == NULL) { + ERROR("HeightMap: Attempted to copy empty height map.\n"); + } else { + memcpy(hlist, cpy->hlist, (rows * columns) * sizeof(HeightVal)); + } +} + +HeightMap::~HeightMap() { + if (hlist != NULL) { + free(hlist); + } +} + +void HeightMap::initMap() { + width = columns * CELL_WIDTH; + height = rows * CELL_HEIGHT; + hlist = (HeightVal *) malloc(sizeof(HeightVal) * rows * columns); +} + +void HeightMap::initMap(float initialOdds) { + width = columns * CELL_WIDTH; + height = rows * CELL_HEIGHT; + hlist = (HeightVal *) malloc(sizeof(HeightVal) * rows * columns); + zeroMap(initialOdds); +} + +void HeightMap::zeroMap(float initialOdds) { + unsigned int count = rows * columns; + for (unsigned int i = 0; i < count; i++) { + hlist[i].height = 0.0; + hlist[i].prob = initialOdds; + hlist[i].observations = 0; + hlist[i].searches = 0; + } +} + +HeightMapArray::HeightMapArray(unsigned int rows, unsigned int columns) : + rows(rows), columns(columns) +{ + array.resize(rows); + for (unsigned int r = 0; r < rows; r++) { + array[r].resize(columns); + } +} + +HeightMultiMap::HeightMultiMap(FastSLAMParameters& parameters) : + parameters(parameters), + rows(parameters.mapRows), columns(parameters.mapColumns), + patches(rows, columns) +{ + + zeroMap(); + offsetX = 0 - (columns * parameters.patchColumns * CELL_WIDTH / 2); + offsetY = 0 - (rows * parameters.patchRows * CELL_HEIGHT / 2); +} + +// performs shallow copy of all non-NULL HeightMaps +HeightMultiMap::HeightMultiMap(HeightMultiMapPtr cpy) : + offsetX(cpy->offsetX), offsetY(cpy->offsetY), + parameters(cpy->parameters), + rows(parameters.mapRows), columns(parameters.mapColumns), + patches(rows, columns) +{ + unsigned int i, j; + + for (i = 0; i < rows; i++) { + for (j = 0; j < columns; j++) { + HeightMapPtr hm = cpy->patches[i][j]; + if (hm != NULL) { + patches[i][j] = hm; + hm->owner = NULL; + } + } + } +} + +HeightMultiMap::~HeightMultiMap() { + releaseSubMaps(); +} + +// set all submaps to NULL +void HeightMultiMap::zeroMap() { + unsigned int i, j; + for (i = 0; i < rows; i++) { + for (j = 0; j < columns; j++) { + HeightMapPtr hm = patches[i][j]; + if (hm!= NULL && hm->owner == this) { + hm->owner = NULL; + } + patches[i][j] = NULL; + } + } +} + +// release ownership of all submaps +void HeightMultiMap::releaseSubMaps() { + unsigned int i, j; + for (i = 0; i < rows; i++) { + for (j = 0; j < columns; j++) { + HeightMapPtr hm = patches[i][j]; + if (hm != NULL && hm->owner == this) { + hm->owner = NULL; + } + } + } +} + double HeightMultiMap::rayTrace(const Point& orig, const Point& dest) { double x = orig.x, y = orig.y, z = orig.z, dx, dy, dz, maxRay, ray; @@ -52,9 +170,11 @@ double HeightMultiMap::rayTrace(const Point& orig, const Point& dest) { x += dx; y += dy; z += dz; ray += RAY_STEP_SIZE; } while (x >= xOrig && y >= yOrig && x < maxX && y < maxY); } else { -// LOG("Unable to locate map for point (%.3lf, %.3lf) in raytrace.\n", -// x, y); - x += dx; y += dy; z += dz; ray += RAY_STEP_SIZE; +// LOG("Unable to locate map for point (%.3lf, %.3lf) in raytrace.\n", x, y); + x += dx; + y += dy; + z += dz; + ray += RAY_STEP_SIZE; } } @@ -104,10 +224,10 @@ void HeightMultiMap::updateTrace(const Point& orig, const Point& dest, double ma if (currentMap != NULL) {{ // Lock lock(currentMap->rwLock); - double xOrig = currentMap->xOrig, - yOrig = currentMap->yOrig, - maxX = xOrig + currentMap->width, - maxY = yOrig + currentMap->height; + double xOrig = currentMap->xOrig; + double yOrig = currentMap->yOrig; + double maxX = xOrig + currentMap->width; + double maxY = yOrig + currentMap->height; HeightVal* prevCell = NULL; do { HeightVal* cell = currentMap->getByXY(x, y); @@ -118,9 +238,9 @@ void HeightMultiMap::updateTrace(const Point& orig, const Point& dest, double ma // Check if cell in search area if (search.maxDist > 0) { - double sdx = x - search.sensor.position.x, - sdy = y - search.sensor.position.y, - d = sqrt(sdx*sdx + sdy*sdy); + double sdx = x - search.sensor.position.x; + double sdy = y - search.sensor.position.y; + double d = sqrt(sdx*sdx + sdy*sdy); if (d <= search.maxDist) { double angle = atan2(sdy, sdx) - search.sensor.orientation; NORMALISE_ANGLE(angle); @@ -135,12 +255,18 @@ void HeightMultiMap::updateTrace(const Point& orig, const Point& dest, double ma } prevCell = cell; - x += dx; y += dy; z += dz; ray += RAY_STEP_SIZE; + x += dx; + y += dy; + z += dz; + ray += RAY_STEP_SIZE; } while (x >= xOrig && y >= yOrig && x < maxX && y < maxY && ray < maxRay && z >= parameters.minHeight && z <= parameters.maxHeight); }} else { LOG("HMM: Unable to get map for point (%.3lf, %.3lf, %.3lf) in updatetrace.\n", x, y, z); - x += dx; y += dy; z += dz; ray += RAY_STEP_SIZE; + x += dx; + y += dy; + z += dz; + ray += RAY_STEP_SIZE; } } } @@ -163,15 +289,17 @@ double HeightMultiMap::getProb(MapCloudPtr cloud, double gain, double maxSensorR // laser points for (i = 0; i < cloud->cloud.size(); i++) { const Point& point = cloud->cloud[i]; - if (!point.isFinite() || point.withinError(sensorPose.position)) + if (!point.isFinite() || point.withinError(sensorPose.position)) { continue; + } dx = point.x - sensorPose.position.x; dy = point.y - sensorPose.position.y; dz = point.z - sensorPose.position.z; d = sqrt(dx*dx + dy*dy + dz*dz); // distance of sensor ray - if (d > maxSensorRange) + if (d > maxSensorRange) { continue; + } res = rayTrace(sensorPose.position, point); @@ -194,8 +322,9 @@ void HeightMultiMap::update(MapCloudPtr cloud, double maxSensorRange, const Sear for (i = 0; i < cloud->cloud.size(); i++) { const Point& point = cloud->cloud[i]; - if (!point.isFinite() || point.withinError(sensorPose.position)) + if (!point.isFinite() || point.withinError(sensorPose.position)) { continue; + } updateTrace(sensorPose.position, point, maxSensorRange, search); } } diff --git a/crosbot_fastslam/src/particle.cpp b/crosbot_fastslam/src/particle.cpp index ca5aff3..f7e55c0 100644 --- a/crosbot_fastslam/src/particle.cpp +++ b/crosbot_fastslam/src/particle.cpp @@ -124,6 +124,26 @@ void Particle::update(MapCloudPtr cloud) { map->update(motionCloud, parameters.maxSensorRange, search); } +PathPtr Particle::getPath() { + Lock lock(rwLock); + PathPtr rval = new Path(); + + for (size_t i = 0; i < history.size(); i++) { + rval->path.push_back(history[i].pose); + if (history[i].cloud != NULL) { + rval->timestamps.push_back(history[i].cloud->timestamp); + } else { + rval->timestamps.push_back(Time(0,0)); + } + } + + if (rval->path.size() == 0 || rval->path[rval->path.size()-1] != pose) { + rval->path.push_back(pose); + } + + return rval; +} + } // namespace fastslam } // namespace crosbot -- GitLab