From 75da2af6b082d21b6815625a09c7ca0982940d9c Mon Sep 17 00:00:00 2001 From: Joel Waite Ziemann Date: Thu, 10 Aug 2023 12:06:14 -0600 Subject: [PATCH] Issue 42 - changes for slow memory leak --- CMakeLists.txt | 4 +- gridfastslam/gridslamprocessor.cpp | 7 +- include/gmapping/grid/array2d.h | 67 +++++++++++-------- include/gmapping/grid/harray2d.h | 53 +++++++++------ .../gmapping/gridfastslam/gridslamprocessor.h | 5 +- .../gridfastslam/gridslamprocessor.hxx | 2 +- .../gmapping/particlefilter/particlefilter.h | 2 + 7 files changed, 83 insertions(+), 57 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eaef201..2d521ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,9 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(openslam_gmapping) ## Find catkin macros and libraries find_package(catkin REQUIRED) +add_compile_options(-std=c++17) + include(GenerateExportHeader) set(EXPORT_HEADER_DIR "${CATKIN_DEVEL_PREFIX}/include") file(MAKE_DIRECTORY "${EXPORT_HEADER_DIR}") diff --git a/gridfastslam/gridslamprocessor.cpp b/gridfastslam/gridslamprocessor.cpp index 51fcf52..e0ec4d5 100644 --- a/gridfastslam/gridslamprocessor.cpp +++ b/gridfastslam/gridslamprocessor.cpp @@ -409,8 +409,8 @@ void GridSlamProcessor::setMotionModelParameters } m_infoStream << "m_count " << m_count << endl; - RangeReading* reading_copy = - new RangeReading(reading.size(), + std::shared_ptr reading_copy = + std::make_shared(reading.size(), &(reading[0]), static_cast(reading.getSensor()), reading.getTime()); @@ -466,7 +466,8 @@ void GridSlamProcessor::setMotionModelParameters updateTreeWeights(false); // cerr << ".done!" < #include #include #include "gmapping/grid/accessstate.h" @@ -11,6 +12,8 @@ namespace GMapping { template class Array2D{ public: + using CellArray = std::shared_ptr; + using CellArray2d = std::shared_ptr; Array2D(int xsize=0, int ysize=0); Array2D& operator=(const Array2D &); Array2D(const Array2D &); @@ -33,10 +36,10 @@ template class Array2D{ inline int getPatchMagnitude() const{return 0;} inline int getXSize() const {return m_xsize;} inline int getYSize() const {return m_ysize;} - inline Cell** cells() {return m_cells;} - Cell ** m_cells; + inline CellArray2d cells() {return m_cells;} + CellArray2d m_cells; protected: - int m_xsize, m_ysize; + int m_xsize = 0, m_ysize = 0; }; @@ -44,12 +47,17 @@ template Array2D::Array2D(int xsize, int ysize){ // assert(xsize>0); // assert(ysize>0); - m_xsize=xsize; - m_ysize=ysize; + // delete memory + if (m_cells) + { + m_cells.reset(); + } + m_xsize=xsize; + m_ysize=ysize; if (m_xsize>0 && m_ysize>0){ - m_cells=new Cell*[m_xsize]; + m_cells=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; }); for (int i=0; i::Array2D(int xsize, int ysize){ template Array2D & Array2D::operator=(const Array2D & g){ if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize){ - for (int i=0; i & Array2D::operator=(const Array2D & template Array2D::Array2D(const Array2D & g){ + // delete memory + if (m_cells) + { + m_cells.reset(); + } m_xsize=g.m_xsize; m_ysize=g.m_ysize; - m_cells=new Cell*[m_xsize]; + m_cells=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; }); for (int x=0; x::~Array2D(){ std::cerr << "m_xsize= " << m_xsize<< std::endl; std::cerr << "m_ysize= " << m_ysize<< std::endl; } - for (int i=0; i @@ -125,12 +139,11 @@ void Array2D::clear(){ std::cerr << "m_xsize= " << m_xsize<< std::endl; std::cerr << "m_ysize= " << m_ysize<< std::endl; } - for (int i=0; i void Array2D::resize(int xmin, int ymin, int xmax, int ymax){ int xsize=xmax-xmin; int ysize=ymax-ymin; - Cell ** newcells=new Cell *[xsize]; + CellArray2d newcells=CellArray2d(new CellArray[xsize], [](auto ptr){ delete[] ptr; }); for (int x=0; x::resize(int xmin, int ymin, int xmax, int ymax){ for (int y=dy; ym_cells[x][y]; } - delete [] this->m_cells[x]; } - delete [] this->m_cells; this->m_cells=newcells; this->m_xsize=xsize; this->m_ysize=ysize; diff --git a/include/gmapping/grid/harray2d.h b/include/gmapping/grid/harray2d.h index 23004ab..4e61c04 100644 --- a/include/gmapping/grid/harray2d.h +++ b/include/gmapping/grid/harray2d.h @@ -1,6 +1,7 @@ #ifndef HARRAY2D_H #define HARRAY2D_H #include +#include #include #include #include "gmapping/grid/array2d.h" @@ -8,8 +9,11 @@ namespace GMapping { template -class HierarchicalArray2D: public Array2D > >{ +class HierarchicalArray2D: public Array2D< std::shared_ptr< Array2D > >{ public: + using HCell = std::shared_ptr< Array2D >; + using HCellArray = std::shared_ptr; + using HCellArray2d = std::shared_ptr; typedef std::set< point, pointcomparator > PointSet; HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5); HierarchicalArray2D(const HierarchicalArray2D& hg); @@ -43,20 +47,27 @@ class HierarchicalArray2D: public Array2D > >{ template HierarchicalArray2D::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude) - :Array2D > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){ + :Array2D > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){ m_patchMagnitude=patchMagnitude; m_patchSize=1<m_xsize = (xsize>>patchMagnitude); + // this->m_ysize = (ysize>>patchMagnitude); } template HierarchicalArray2D::HierarchicalArray2D(const HierarchicalArray2D& hg) - :Array2D > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude)) // added by cyrill: if you have a resize error, check this again + :Array2D > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude)) // added by cyrill: if you have a resize error, check this again { this->m_xsize=hg.m_xsize; this->m_ysize=hg.m_ysize; - this->m_cells=new autoptr< Array2D >*[this->m_xsize]; + if (this->m_cells) + { + this->m_cells.reset(); + } + // the Array2D m_cells is a std::shared_ptr[]>, where Cell is std::shared_ptr< Array2D > + this->m_cells=HCellArray2d(new HCellArray[this->m_xsize], [](auto ptr){ delete[] ptr; }); for (int x=0; xm_xsize; x++){ - this->m_cells[x]=new autoptr< Array2D >[this->m_ysize]; + this->m_cells[x]=HCellArray(new HCell[this->m_ysize]); for (int y=0; ym_ysize; y++) this->m_cells[x][y]=hg.m_cells[x][y]; } @@ -68,11 +79,11 @@ template void HierarchicalArray2D::resize(int xmin, int ymin, int xmax, int ymax){ int xsize=xmax-xmin; int ysize=ymax-ymin; - autoptr< Array2D > ** newcells=new autoptr< Array2D > *[xsize]; + HCellArray2d newcells=HCellArray2d(new HCellArray[xsize], [](auto ptr){ delete[] ptr; }); for (int x=0; x >[ysize]; + newcells[x]=HCellArray(new HCell[ysize], [](auto ptr){ delete[] ptr; }); for (int y=0; y >(0); + newcells[x][y]=HCell(); } } int dx= xmin < 0 ? 0 : xmin; @@ -83,9 +94,7 @@ void HierarchicalArray2D::resize(int xmin, int ymin, int xmax, int ymax){ for (int y=dy; ym_cells[x][y]; } - delete [] this->m_cells[x]; } - delete [] this->m_cells; this->m_cells=newcells; this->m_xsize=xsize; this->m_ysize=ysize; @@ -95,14 +104,12 @@ template HierarchicalArray2D& HierarchicalArray2D::operator=(const HierarchicalArray2D& hg){ // Array2D > >::operator=(hg); if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){ - for (int i=0; im_xsize; i++) - delete [] this->m_cells[i]; - delete [] this->m_cells; + this->m_cells.reset(); this->m_xsize=hg.m_xsize; this->m_ysize=hg.m_ysize; - this->m_cells=new autoptr< Array2D >*[this->m_xsize]; + this->m_cells=HCellArray2d(new HCellArray[this->m_xsize], [](auto ptr){ delete[] ptr; }); for (int i=0; im_xsize; i++) - this->m_cells[i]=new autoptr< Array2D > [this->m_ysize]; + this->m_cells[i]=HCellArray(new HCell[this->m_ysize]); } for (int x=0; xm_xsize; x++) for (int y=0; ym_ysize; y++) @@ -137,32 +144,34 @@ Array2D* HierarchicalArray2D::createPatch(const IntPoint& ) const{ template AccessibilityState HierarchicalArray2D::cellState(int x, int y) const { if (this->isInside(patchIndexes(x,y))) + { if(isAllocated(x,y)) return (AccessibilityState)((int)Inside|(int)Allocated); else return Inside; + } return Outside; } template void HierarchicalArray2D::allocActiveArea(){ for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); it++){ - const autoptr< Array2D >& ptr=this->m_cells[it->x][it->y]; + const HCell ptr=this->m_cells[it->x][it->y]; Array2D* patch=0; if (!ptr){ patch=createPatch(*it); } else{ patch=new Array2D(*ptr); } - this->m_cells[it->x][it->y]=autoptr< Array2D >(patch); + this->m_cells[it->x][it->y]=HCell(patch); } } template bool HierarchicalArray2D::isAllocated(int x, int y) const{ IntPoint c=patchIndexes(x,y); - autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; - return (ptr != 0); + HCell ptr=this->m_cells[c.x][c.y]; + return (ptr.get() != nullptr); } template @@ -178,10 +187,10 @@ Cell& HierarchicalArray2D::cell(int x, int y){ assert(this->isInside(c.x, c.y)); if (!this->m_cells[c.x][c.y]){ Array2D* patch=createPatch(IntPoint(x,y)); - this->m_cells[c.x][c.y]=autoptr< Array2D >(patch); + this->m_cells[c.x][c.y]=HCell(patch); //cerr << "!!! FATAL: your dick is going to fall down" << endl; } - autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; + HCell ptr=this->m_cells[c.x][c.y]; return (*ptr).cell(IntPoint(x-(c.x< const Cell& HierarchicalArray2D::cell(int x, int y) const{ assert(isAllocated(x,y)); IntPoint c=patchIndexes(x,y); - const autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; + const HCell ptr=this->m_cells[c.x][c.y]; return (*ptr).cell(IntPoint(x-(c.x< #include #include +#include #include #include #include @@ -70,7 +71,7 @@ namespace GMapping { TNode* parent; /**The range reading to which this node is associated*/ - const RangeReading* reading; + std::shared_ptr reading; /**The number of childs*/ unsigned int childs; @@ -318,7 +319,7 @@ namespace GMapping { // return if a resampling occured or not inline bool resample(const double* plainReading, int adaptParticles, - const RangeReading* rr=0); + std::shared_ptr rr=std::shared_ptr()); //tree utilities diff --git a/include/gmapping/gridfastslam/gridslamprocessor.hxx b/include/gmapping/gridfastslam/gridslamprocessor.hxx index 8a8b7a4..d6c1ef5 100644 --- a/include/gmapping/gridfastslam/gridslamprocessor.hxx +++ b/include/gmapping/gridfastslam/gridslamprocessor.hxx @@ -67,7 +67,7 @@ inline void GridSlamProcessor::normalize(){ } -inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){ +inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, std::shared_ptr reading){ bool hasResampled = false; diff --git a/include/gmapping/particlefilter/particlefilter.h b/include/gmapping/particlefilter/particlefilter.h index 5823525..1e72bad 100644 --- a/include/gmapping/particlefilter/particlefilter.h +++ b/include/gmapping/particlefilter/particlefilter.h @@ -139,8 +139,10 @@ void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){ } } if (count>0) + { *out=std::make_pair(current,count); out++; + } } //BEGIN legacy