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/LICENSE b/LICENSE
new file mode 100644
index 0000000..0c01048
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,26 @@
+Copyright (c) 2019 Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Willow Garage, Inc. nor the names of its
+      contributors may be used to endorse or promote products derived from
+      this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
diff --git a/gridfastslam/gfs2log.cpp b/gridfastslam/gfs2log.cpp
index e688e77..f0066c3 100644
--- a/gridfastslam/gfs2log.cpp
+++ b/gridfastslam/gfs2log.cpp
@@ -15,12 +15,11 @@ using namespace GMapping::GFSReader;
 
 int main (int argc, const char * const * argv){
 	if (argc<3){
-		cout << "usage gfs2log [-err] [-neff] [-part] [-odom] <infilename> <outfilename>" << endl;
+		cout << "usage gfs2log [-err] [-part] [-odom] <infilename> <outfilename>" << endl;
 		cout << "  -odom : dump raw odometry in ODOM message instead of inpolated corrected one" << endl;
 		return -1;
 	}
 	bool err=0;
-	bool neff=0;
 	bool part=0;
 	bool odom=0;
 	//	int particle_num;
@@ -29,10 +28,6 @@ int main (int argc, const char * const * argv){
 		err=true;
 		c++;
 	}
-	if (!strcmp(argv[c],"-neff")){
-		neff=true;
-		c++;
-	}
 	if (!strcmp(argv[c],"-part")){
 		part=true;
 		c++;
diff --git a/gridfastslam/gfs2rec.cpp b/gridfastslam/gfs2rec.cpp
index 5eb65fa..fb0ccda 100644
--- a/gridfastslam/gfs2rec.cpp
+++ b/gridfastslam/gfs2rec.cpp
@@ -375,16 +375,11 @@ int main (int argc, const char * const * argv){
 		return -1;
 	}
 	bool err=0;
-	bool neff=0;
 	unsigned int  c=1;
 	if (!strcmp(argv[c],"-err")){
 		err=true;
 		c++;
 	}
-	if (!strcmp(argv[c],"-neff")){
-		neff=true;
-		c++;
-	}
 	ifstream is(argv[c]);
 	if (!is){
 		cout << "could read file "<< endl;
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<RangeReading> reading_copy =
+              std::make_shared<RangeReading>(reading.size(),
                                &(reading[0]),
                                static_cast<const RangeSensor*>(reading.getSensor()),
                                reading.getTime());
@@ -466,7 +466,8 @@ void GridSlamProcessor::setMotionModelParameters
       updateTreeWeights(false);
       //		cerr  << ".done!" <<endl;
       
-      delete [] plainReading;
+      delete[] plainReading;
+      plainReading=0;
       m_lastPartPose=m_odoPose; //update the past pose for the next iteration
       m_linearDistance=0;
       m_angularDistance=0;
diff --git a/include/gmapping/grid/array2d.h b/include/gmapping/grid/array2d.h
index 0d0c4eb..c48e48e 100644
--- a/include/gmapping/grid/array2d.h
+++ b/include/gmapping/grid/array2d.h
@@ -1,6 +1,7 @@
 #ifndef ARRAY2D_H
 #define ARRAY2D_H
 
+#include <memory>
 #include <assert.h>
 #include <gmapping/utils/point.h>
 #include "gmapping/grid/accessstate.h"
@@ -11,6 +12,8 @@ namespace GMapping {
 
 template<class Cell, const bool debug=false> class Array2D{
 	public:
+    using CellArray = std::shared_ptr<Cell[]>;
+    using CellArray2d = std::shared_ptr<CellArray[]>;
 		Array2D(int xsize=0, int ysize=0);
 		Array2D& operator=(const Array2D &);
 		Array2D(const Array2D<Cell,debug> &);
@@ -33,10 +36,10 @@ template<class Cell, const bool debug=false> 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,18 +47,29 @@ template <class Cell, const bool debug>
 Array2D<Cell,debug>::Array2D(int xsize, int ysize){
 //	assert(xsize>0);
 //	assert(ysize>0);
-	m_xsize=xsize;
-	m_ysize=ysize;
-	if (m_xsize>0 && m_ysize>0){
-		m_cells=new Cell*[m_xsize];
+  // delete memory
+  if (m_cells)
+  {
+    m_cells.reset();
+  }
+  m_xsize=xsize;
+  m_ysize=ysize;
+	if (m_xsize>0 && m_ysize>0)
+  {
+    m_cells=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; });
 		for (int i=0; i<m_xsize; i++)
-			m_cells[i]=new Cell[m_ysize];
+    {
+      m_cells[i]=CellArray(new Cell[m_ysize], [](auto ptr){ delete[] ptr; });
+    }
 	}
-	else{
+	else
+  {
+    std::cerr << __func__ << "Invalid size: " << "m_xsize= " << m_xsize << " m_ysize= " << m_ysize << " - resetting " << std::endl;
 		m_xsize=m_ysize=0;
-		m_cells=0;
+    m_cells.reset();
 	}
-	if (debug){
+	if (debug)
+  {
 		std::cerr << __func__ << std::endl;
 		std::cerr << "m_xsize= " << m_xsize<< std::endl;
 		std::cerr << "m_ysize= " << m_ysize<< std::endl;
@@ -64,21 +78,40 @@ Array2D<Cell,debug>::Array2D(int xsize, int ysize){
 
 template <class Cell, const bool debug>
 Array2D<Cell,debug> & Array2D<Cell,debug>::operator=(const Array2D<Cell,debug> & g){
-	if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize){
-		for (int i=0; i<m_xsize; i++)
-			delete [] m_cells[i];
-		delete [] m_cells;
+	if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize)
+  {
+    // delete memory
+    if (m_cells)
+    {
+      m_cells.reset();
+    }
 		m_xsize=g.m_xsize;
 		m_ysize=g.m_ysize;
-		m_cells=new Cell*[m_xsize];
-		for (int i=0; i<m_xsize; i++)
-			m_cells[i]=new Cell[m_ysize];
+    if (m_xsize > 0 && m_ysize > 0)
+    {
+      m_cells = CellArray2d(new CellArray[m_xsize], [](auto ptr) { delete[] ptr; });
+      for (int i = 0; i < m_xsize; i++)
+      {
+        m_cells[i] = CellArray(new Cell[m_ysize], [](auto ptr) { delete[] ptr; });
+      }
+    }
+    else
+    {
+      std::cerr << __func__ << "Invalid size: " << "m_xsize= " << m_xsize << " m_ysize= " << m_ysize << " - resetting " << std::endl;
+      m_xsize = m_ysize = 0;
+      m_cells.reset();
+    }
 	}
 	for (int x=0; x<m_xsize; x++)
-		for (int y=0; y<m_ysize; y++)
-			m_cells[x][y]=g.m_cells[x][y];
+  {
+    for (int y = 0; y < m_ysize; y++)
+    {
+      m_cells[x][y] = g.m_cells[x][y];
+    }
+  }
 	
-	if (debug){
+	if (debug)
+  {
 		std::cerr << __func__ << std::endl;
 		std::cerr << "m_xsize= " << m_xsize<< std::endl;
 		std::cerr << "m_ysize= " << m_ysize<< std::endl;
@@ -87,15 +120,33 @@ Array2D<Cell,debug> & Array2D<Cell,debug>::operator=(const Array2D<Cell,debug> &
 }
 
 template <class Cell, const bool debug>
-Array2D<Cell,debug>::Array2D(const Array2D<Cell,debug> & g){
+Array2D<Cell,debug>::Array2D(const Array2D<Cell,debug> & 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];
-	for (int x=0; x<m_xsize; x++){
-		m_cells[x]=new Cell[m_ysize];
-		for (int y=0; y<m_ysize; y++)
-			m_cells[x][y]=g.m_cells[x][y];
-	}
+  if (m_xsize > 0 && m_ysize > 0)
+  {
+    m_cells = CellArray2d(new CellArray[m_xsize], [](auto ptr) { delete[] ptr; });
+    for (int x = 0; x < m_xsize; x++)
+    {
+      m_cells[x] = CellArray(new Cell[m_ysize], [](auto ptr) { delete[] ptr; });
+      for (int y = 0; y < m_ysize; y++)
+      {
+        m_cells[x][y] = g.m_cells[x][y];
+      }
+    }
+  }
+  else
+  {
+    std::cerr << __func__ << "Invalid size: " << "m_xsize= " << m_xsize << " m_ysize= " << m_ysize << " - resetting " << std::endl;
+    m_xsize=m_ysize=0;
+    m_cells.reset();
+  }
 	if (debug){
 		std::cerr << __func__ << std::endl;
 		std::cerr << "m_xsize= " << m_xsize<< std::endl;
@@ -107,30 +158,28 @@ template <class Cell, const bool debug>
 Array2D<Cell,debug>::~Array2D(){
   if (debug){
   	std::cerr << __func__ << std::endl;
-	std::cerr << "m_xsize= " << m_xsize<< std::endl;
-	std::cerr << "m_ysize= " << m_ysize<< std::endl;
+    std::cerr << "m_xsize= " << m_xsize<< std::endl;
+    std::cerr << "m_ysize= " << m_ysize<< std::endl;
   }
-  for (int i=0; i<m_xsize; i++){
-    delete [] m_cells[i];
-    m_cells[i]=0;
+  // delete memory
+  if (m_cells)
+  {
+    m_cells.reset();
   }
-  delete [] m_cells;
-  m_cells=0;
 }
 
 template <class Cell, const bool debug>
 void Array2D<Cell,debug>::clear(){
   if (debug){
   	std::cerr << __func__ << std::endl;
-	std::cerr << "m_xsize= " << m_xsize<< std::endl;
-	std::cerr << "m_ysize= " << m_ysize<< std::endl;
+    std::cerr << "m_xsize= " << m_xsize<< std::endl;
+    std::cerr << "m_ysize= " << m_ysize<< std::endl;
   }
-  for (int i=0; i<m_xsize; i++){
-    delete [] m_cells[i];
-    m_cells[i]=0;
+  // delete memory
+  if (m_cells)
+  {
+    m_cells.reset();
   }
-  delete [] m_cells;
-  m_cells=0;
   m_xsize=0;
   m_ysize=0;
 }
@@ -138,11 +187,16 @@ void Array2D<Cell,debug>::clear(){
 
 template <class Cell, const bool debug>
 void Array2D<Cell,debug>::resize(int xmin, int ymin, int xmax, int ymax){
-	int xsize=xmax-xmin;
-	int ysize=ymax-ymin;
-	Cell ** newcells=new Cell *[xsize];
+	int xsize = xmax-xmin;
+	int ysize = ymax-ymin;
+  if (xsize <= 0 || ysize <= 0)
+  {
+    std::cerr << __func__ << "Invalid reset size: " << "xsize= " << xsize << " ysize= " << ysize << " - no-op " << std::endl;
+    return;
+  }
+  CellArray2d newcells=CellArray2d(new CellArray[xsize], [](auto ptr){ delete[] ptr; });
 	for (int x=0; x<xsize; x++){
-		newcells[x]=new Cell[ysize];
+    newcells[x]=CellArray(new Cell[ysize], [](auto ptr){ delete[] ptr; });;
 	}
 	int dx= xmin < 0 ? 0 : xmin;
 	int dy= ymin < 0 ? 0 : ymin;
@@ -152,9 +206,7 @@ void Array2D<Cell,debug>::resize(int xmin, int ymin, int xmax, int ymax){
 		for (int y=dy; y<Dy; y++){
 			newcells[x-xmin][y-ymin]=this->m_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..1aaebc1 100644
--- a/include/gmapping/grid/harray2d.h
+++ b/include/gmapping/grid/harray2d.h
@@ -1,6 +1,7 @@
 #ifndef HARRAY2D_H
 #define HARRAY2D_H
 #include <set>
+#include <memory>
 #include <gmapping/utils/point.h>
 #include <gmapping/utils/autoptr.h>
 #include "gmapping/grid/array2d.h"
@@ -8,8 +9,11 @@
 namespace GMapping {
 
 template <class Cell>
-class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
+class HierarchicalArray2D: public Array2D< std::shared_ptr< Array2D<Cell> > >{
 	public:
+    using HCell = std::shared_ptr< Array2D<Cell> >;
+    using HCellArray = std::shared_ptr<HCell[]>;
+    using HCellArray2d = std::shared_ptr<HCellArray[]>;
 		typedef std::set< point<int>, pointcomparator<int> > PointSet;
 		HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5);
 		HierarchicalArray2D(const HierarchicalArray2D& hg);
@@ -43,49 +47,75 @@ class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
 
 template <class Cell>
 HierarchicalArray2D<Cell>::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude) 
-  :Array2D<autoptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
+  :Array2D<std::shared_ptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
 	m_patchMagnitude=patchMagnitude;
 	m_patchSize=1<<m_patchMagnitude;
+    // this->m_xsize = (xsize>>patchMagnitude);
+    // this->m_ysize = (ysize>>patchMagnitude);
 }
 
 template <class Cell>
 HierarchicalArray2D<Cell>::HierarchicalArray2D(const HierarchicalArray2D& hg)
-  :Array2D<autoptr< Array2D<Cell> > >::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<std::shared_ptr< Array2D<Cell> > >::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<Cell> >*[this->m_xsize];
-	for (int x=0; x<this->m_xsize; x++){
-		this->m_cells[x]=new autoptr< Array2D<Cell> >[this->m_ysize];
-		for (int y=0; y<this->m_ysize; y++)
-			this->m_cells[x][y]=hg.m_cells[x][y];
-	}
-	this->m_patchMagnitude=hg.m_patchMagnitude;
-	this->m_patchSize=hg.m_patchSize;
+  if (this->m_cells)
+  {
+    this->m_cells.reset();
+  }
+  if (this->m_xsize > 0 && this->m_ysize > 0)
+  {
+    // the Array2D m_cells is a std::shared_ptr<std::shared_ptr<Cell[]>[]>, where Cell is std::shared_ptr< Array2D<Cell> >
+    this->m_cells = HCellArray2d(new HCellArray[this->m_xsize], [](auto ptr) { delete[] ptr; });
+    for (int x = 0; x < this->m_xsize; x++)
+    {
+      this->m_cells[x] = HCellArray(new HCell[this->m_ysize]);
+      for (int y = 0; y < this->m_ysize; y++)
+      {
+        this->m_cells[x][y] = hg.m_cells[x][y];
+      }
+    }
+    this->m_patchMagnitude = hg.m_patchMagnitude;
+    this->m_patchSize = hg.m_patchSize;
+  }
+  else
+  {
+    std::cerr << __func__ << "Invalid size: " << "m_xsize= " << this->m_xsize << " m_ysize= " << this->m_ysize << " - resetting " << std::endl;
+    this->m_xsize = this->m_ysize=0;
+    this->m_cells.reset();
+  }
 }
 
 template <class Cell>
 void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
 	int xsize=xmax-xmin;
 	int ysize=ymax-ymin;
-	autoptr< Array2D<Cell> > ** newcells=new autoptr< Array2D<Cell> > *[xsize];
-	for (int x=0; x<xsize; x++){
-		newcells[x]=new autoptr< Array2D<Cell> >[ysize];
-		for (int y=0; y<ysize; y++){
-			newcells[x][y]=autoptr< Array2D<Cell> >(0);
+  if (xsize <= 0 || ysize <= 0)
+  {
+    std::cerr << __func__ << "Invalid reset size: " << "xsize= " << xsize << " ysize= " << ysize << " - no-op " << std::endl;
+    return;
+  }
+  HCellArray2d newcells=HCellArray2d(new HCellArray[xsize], [](auto ptr){ delete[] ptr; });
+	for (int x=0; x<xsize; x++)
+  {
+    newcells[x]=HCellArray(new HCell[ysize], [](auto ptr){ delete[] ptr; });
+		for (int y=0; y<ysize; y++)
+    {
+      newcells[x][y]=HCell();
 		}
 	}
 	int dx= xmin < 0 ? 0 : xmin;
 	int dy= ymin < 0 ? 0 : ymin;
 	int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
 	int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
-	for (int x=dx; x<Dx; x++){
-		for (int y=dy; y<Dy; y++){
+	for (int x=dx; x<Dx; x++)
+  {
+		for (int y=dy; y<Dy; y++)
+    {
 			newcells[x-xmin][y-ymin]=this->m_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 +125,24 @@ template <class Cell>
 HierarchicalArray2D<Cell>& HierarchicalArray2D<Cell>::operator=(const HierarchicalArray2D& hg){
 //	Array2D<autoptr< Array2D<Cell> > >::operator=(hg);
 	if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){
-		for (int i=0; i<this->m_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<Cell> >*[this->m_xsize];
-		for (int i=0; i<this->m_xsize; i++)
-			this->m_cells[i]=new autoptr< Array2D<Cell> > [this->m_ysize];
+
+    if (this->m_xsize > 0 && this->m_ysize > 0)
+    {
+      this->m_cells = HCellArray2d(new HCellArray[this->m_xsize], [](auto ptr) { delete[] ptr; });
+      for (int i = 0; i < this->m_xsize; i++)
+      {
+        this->m_cells[i] = HCellArray(new HCell[this->m_ysize]);
+      }
+    }
+    else
+    {
+      std::cerr << __func__ << "Invalid size: " << "m_xsize= " << this->m_xsize << " m_ysize= " << this->m_ysize << " - resetting " << std::endl;
+      this->m_xsize = this->m_ysize = 0;
+      this->m_cells.reset();
+    }
 	}
 	for (int x=0; x<this->m_xsize; x++)
 		for (int y=0; y<this->m_ysize; y++)
@@ -137,32 +177,34 @@ Array2D<Cell>* HierarchicalArray2D<Cell>::createPatch(const IntPoint& ) const{
 template <class Cell>
 AccessibilityState  HierarchicalArray2D<Cell>::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 <class Cell>
 void HierarchicalArray2D<Cell>::allocActiveArea(){
 	for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); it++){
-		const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y];
+		const HCell ptr=this->m_cells[it->x][it->y];
 		Array2D<Cell>* patch=0;
 		if (!ptr){
 			patch=createPatch(*it);
 		} else{	
 			patch=new Array2D<Cell>(*ptr);
 		}
-		this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);
+		this->m_cells[it->x][it->y]=HCell(patch);
 	}
 }
 
 template <class Cell>
 bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const{
 	IntPoint c=patchIndexes(x,y);
-	autoptr< Array2D<Cell> >& 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 <class Cell>
@@ -178,10 +220,10 @@ Cell& HierarchicalArray2D<Cell>::cell(int x, int y){
 	assert(this->isInside(c.x, c.y));
 	if (!this->m_cells[c.x][c.y]){
 		Array2D<Cell>* patch=createPatch(IntPoint(x,y));
-		this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
+    this->m_cells[c.x][c.y]=HCell(patch);
 		//cerr << "!!! FATAL: your dick is going to fall down" << endl;
 	}
-	autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
+	HCell ptr=this->m_cells[c.x][c.y];
 	return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
 }
 
@@ -189,7 +231,7 @@ template <class Cell>
 const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const{
 	assert(isAllocated(x,y));
 	IntPoint c=patchIndexes(x,y);
-	const autoptr< Array2D<Cell> >& 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<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
 }
 
diff --git a/include/gmapping/grid/map.h b/include/gmapping/grid/map.h
index 1d91248..1efa370 100644
--- a/include/gmapping/grid/map.h
+++ b/include/gmapping/grid/map.h
@@ -190,7 +190,7 @@ Point Map<Cell,Storage,isClass>::map2world(const IntPoint& p) const{
 template <class Cell, class Storage, const bool isClass>
 Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) {
 	AccessibilityState s=m_storage.cellState(p);
-	if (! s&Inside)
+	if ( !(s&Inside) )
 		assert(0);
 	//if (s&Allocated) return m_storage.cell(p); assert(0);
 
diff --git a/include/gmapping/gridfastslam/gridslamprocessor.h b/include/gmapping/gridfastslam/gridslamprocessor.h
index 6595b51..ba841bb 100644
--- a/include/gmapping/gridfastslam/gridslamprocessor.h
+++ b/include/gmapping/gridfastslam/gridslamprocessor.h
@@ -5,6 +5,7 @@
 #include <limits>
 #include <fstream>
 #include <vector>
+#include <memory>
 #include <deque>
 #include <gmapping/particlefilter/particlefilter.h>
 #include <gmapping/utils/point.h>
@@ -70,7 +71,7 @@ namespace GMapping {
       TNode* parent;
 
       /**The range reading to which this node is associated*/
-      const RangeReading* reading;
+      std::shared_ptr<RangeReading> 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<RangeReading> rr=std::shared_ptr<RangeReading>());
     
     //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<RangeReading> 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
diff --git a/scanmatcher/scanmatch_test.cpp b/scanmatcher/scanmatch_test.cpp
index a2fc94a..e7bdb78 100644
--- a/scanmatcher/scanmatch_test.cpp
+++ b/scanmatcher/scanmatch_test.cpp
@@ -165,6 +165,10 @@ int main(int argc, const char * const * argv){
 			}
 			const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
 			assert (rs && rs->beams().size()==rr->size());
+            if (!rs || rs->beams().size() != rr->size()){
+                cout << "Invalid RangeSensor." << endl;
+                return -1;
+            }
 			odopathStream << rr->getPose().x << " " << rr->getPose().y << endl;
 			scanmatcher.processScan(*rr);
 			OrientedPoint p=scanmatcher.getPose();
diff --git a/scanmatcher/scanmatcher.cpp b/scanmatcher/scanmatcher.cpp
index f7fb885..3a82af3 100644
--- a/scanmatcher/scanmatcher.cpp
+++ b/scanmatcher/scanmatcher.cpp
@@ -170,7 +170,7 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
 			if (d>m_usableRange)
 				d=m_usableRange;
 			Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
-			IntPoint p0=map.world2map(lp);
+			p0=map.world2map(lp);
 			IntPoint p1=map.world2map(phit);
 			
 			//IntPoint linePoints[20000] ;
@@ -432,7 +432,7 @@ double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix
 	double bestScore=-1;
 	OrientedPoint currentPose=init;
 	ScoredMove sm={currentPose,0,0};
-	unsigned int matched=likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
+    (void)likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
 	double currentScore=sm.score;
 	moveList.push_back(sm);
 	double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
@@ -497,7 +497,7 @@ double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix
 			localScore=odo_gain*score(map, localPose, readings);
 			//update the score
 			count++;
-			matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);
+            (void)likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);
 			if (localScore>currentScore){
 				currentScore=localScore;
 				bestLocalPose=localPose;
diff --git a/sensor/sensor_range/rangereading.cpp b/sensor/sensor_range/rangereading.cpp
index a95e3c7..c1ee376 100644
--- a/sensor/sensor_range/rangereading.cpp
+++ b/sensor/sensor_range/rangereading.cpp
@@ -58,9 +58,10 @@ unsigned int RangeReading::rawView(double* v, double density) const{
 };
 
 unsigned int RangeReading::activeBeams(double density) const{
-	if (density==0.)
+	if (density==0.){
 		return size();
-		int ab=0;
+    }
+	int ab=0;
 	Point lastPoint(0,0);
 	uint suppressed=0;
 	for (unsigned int i=0; i<size(); i++){