diff --git a/include/gmapping/scanmatcher/scanmatcher.h b/include/gmapping/scanmatcher/scanmatcher.h
index d9d4871..170c99b 100644
--- a/include/gmapping/scanmatcher/scanmatcher.h
+++ b/include/gmapping/scanmatcher/scanmatcher.h
@@ -8,7 +8,7 @@
 #include <iostream>
 #include <gmapping/utils/gvalues.h>
 #include <gmapping/scanmatcher/scanmatcher_export.h>
-#define LASER_MAXBEAMS 2048
+#include <vector>
 
 namespace GMapping {
 
@@ -35,7 +35,7 @@ class SCANMATCHER_EXPORT ScanMatcher{
 		inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
 		double likelihood(double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
 		double likelihood(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180.);
-		inline const double* laserAngles() const { return m_laserAngles; }
+		inline const double* laserAngles() const { return m_laserAngles.data(); }
 		inline unsigned int laserBeams() const { return m_laserBeams; }
 		
 		static const double nullLikelihood;
@@ -45,7 +45,7 @@ class SCANMATCHER_EXPORT ScanMatcher{
 		
 		/**laser parameters*/
 		unsigned int m_laserBeams;
-		double       m_laserAngles[LASER_MAXBEAMS];
+		std::vector<double>       m_laserAngles;
 		//OrientedPoint m_laserPose;
 		PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
 		PARAM_SET_GET(double, laserMaxRange, protected, public, public)
@@ -75,7 +75,7 @@ class SCANMATCHER_EXPORT ScanMatcher{
 };
 
 inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
-	const double * angle=m_laserAngles+m_initialBeamsSkip;
+	std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
 	OrientedPoint lp=p;
 	lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
 	lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
@@ -90,12 +90,12 @@ inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& m
 		if (*r>m_usableRange||*r==0.0) continue;
 		if (skip) continue;
 		Point phit=lp;
-		phit.x+=*r*cos(lp.theta+*angle);
-		phit.y+=*r*sin(lp.theta+*angle);
+		phit.x+=*r*cos(lp.theta+(*angle));
+		phit.y+=*r*sin(lp.theta+(*angle));
 		IntPoint iphit=map.world2map(phit);
 		Point pfree=lp;
-		pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
-		pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
+		pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+(*angle));
+		pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+(*angle));
  		pfree=pfree-phit;
 		IntPoint ipfree=map.world2map(pfree);
 		bool found=false;
@@ -143,7 +143,7 @@ inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& m
 
 inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
 	double s=0;
-	const double * angle=m_laserAngles+m_initialBeamsSkip;
+	std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
 	OrientedPoint lp=p;
 	lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
 	lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
@@ -155,12 +155,12 @@ inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint&
 		skip=skip>m_likelihoodSkip?0:skip;
 		if (skip||*r>m_usableRange||*r==0.0) continue;
 		Point phit=lp;
-		phit.x+=*r*cos(lp.theta+*angle);
-		phit.y+=*r*sin(lp.theta+*angle);
+		phit.x+=*r*cos(lp.theta+(*angle));
+		phit.y+=*r*sin(lp.theta+(*angle));
 		IntPoint iphit=map.world2map(phit);
 		Point pfree=lp;
-		pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
-		pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
+		pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+(*angle));
+		pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+(*angle));
  		pfree=pfree-phit;
 		IntPoint ipfree=map.world2map(pfree);
 		bool found=false;
@@ -193,7 +193,7 @@ inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const
 	using namespace std;
 	l=0;
 	s=0;
-	const double * angle=m_laserAngles+m_initialBeamsSkip;
+	std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
 	OrientedPoint lp=p;
 	lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
 	lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
@@ -208,12 +208,12 @@ inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const
 		if (*r>m_usableRange) continue;
 		if (skip) continue;
 		Point phit=lp;
-		phit.x+=*r*cos(lp.theta+*angle);
-		phit.y+=*r*sin(lp.theta+*angle);
+		phit.x+=*r*cos(lp.theta+(*angle));
+		phit.y+=*r*sin(lp.theta+(*angle));
 		IntPoint iphit=map.world2map(phit);
 		Point pfree=lp;
-		pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
-		pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
+		pfree.x+=(*r-freeDelta)*cos(lp.theta+(*angle));
+		pfree.y+=(*r-freeDelta)*sin(lp.theta+(*angle));
 		pfree=pfree-phit;
 		IntPoint ipfree=map.world2map(pfree);
 		bool found=false;
diff --git a/scanmatcher/scanmatcher.cpp b/scanmatcher/scanmatcher.cpp
index f7fb885..1480108 100644
--- a/scanmatcher/scanmatcher.cpp
+++ b/scanmatcher/scanmatcher.cpp
@@ -131,13 +131,13 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
 	if (lp.y>max.y) max.y=lp.y;
 	
 	/*determine the size of the area*/
-	const double * angle=m_laserAngles+m_initialBeamsSkip;
+	std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
 	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
 		if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
 		double d=*r>m_usableRange?m_usableRange:*r;
 		Point phit=lp;
-		phit.x+=d*cos(lp.theta+*angle);
-		phit.y+=d*sin(lp.theta+*angle);
+		phit.x+=d*cos(lp.theta+(*angle));
+		phit.y+=d*sin(lp.theta+(*angle));
 		if (phit.x<min.x) min.x=phit.x;
 		if (phit.y<min.y) min.y=phit.y;
 		if (phit.x>max.x) max.x=phit.x;
@@ -161,7 +161,7 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
 	
 	HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
 	/*allocate the active area*/
-	angle=m_laserAngles+m_initialBeamsSkip;
+	angle=m_laserAngles.begin()+m_initialBeamsSkip;
 	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
 		if (m_generateMap){
 			double d=*r;
@@ -169,7 +169,7 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
 				continue;
 			if (d>m_usableRange)
 				d=m_usableRange;
-			Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
+			Point phit=lp+Point(d*cos(lp.theta+(*angle)),d*sin(lp.theta+(*angle)));
 			IntPoint p0=map.world2map(lp);
 			IntPoint p1=map.world2map(phit);
 			
@@ -226,7 +226,7 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
 	IntPoint p0=map.world2map(lp);
 	
 	
-	const double * angle=m_laserAngles+m_initialBeamsSkip;
+	std::vector<double>::const_iterator angle=m_laserAngles.begin()+m_initialBeamsSkip;
 	double esum=0;
 	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
 		if (m_generateMap){
@@ -235,7 +235,7 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
 				continue;
 			if (d>m_usableRange)
 				d=m_usableRange;
-			Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
+			Point phit=lp+Point(d*cos(lp.theta+(*angle)),d*sin(lp.theta+(*angle)));
 			IntPoint p1=map.world2map(phit);
 			//IntPoint linePoints[20000] ;
 			GridLineTraversalLine line;
@@ -257,8 +257,8 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
 		} else {
 			if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
 			Point phit=lp;
-			phit.x+=*r*cos(lp.theta+*angle);
-			phit.y+=*r*sin(lp.theta+*angle);
+			phit.x+=*r*cos(lp.theta+(*angle));
+			phit.y+=*r*sin(lp.theta+(*angle));
 			IntPoint p1=map.world2map(phit);
 			assert(p1.x>=0 && p1.y>=0);
 			map.cell(p1).update(true,phit);
@@ -560,11 +560,10 @@ void ScanMatcher::setLaserParameters
 	/*if (m_laserAngles)
 		delete [] m_laserAngles;
 	*/
-	assert(beams<LASER_MAXBEAMS);
 	m_laserPose=lpose;
 	m_laserBeams=beams;
 	//m_laserAngles=new double[beams];
-	memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);	
+	m_laserAngles = vector<double>(angles, angles+beams);
 }