add slam_gmapping

slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt

@@ -0,0 +1,11 @@
+add_library(gridfastslam
+        gfsreader.cpp
+        gridslamprocessor.cpp
+        gridslamprocessor_tree.cpp
+        motionmodel.cpp
+        )
+target_link_libraries(gridfastslam scanmatcher sensor_range)
+
+install(TARGETS gridfastslam DESTINATION lib)
+
+#ament_export_libraries(gridfastslam)

slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp

@@ -0,0 +1,64 @@
+#include <cstring>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <list>
+#include <gmapping/utils/point.h>
+#include "gfsreader.h"
+
+#define MAX_LINE_LENGHT (1000000)
+
+using namespace std;
+using namespace GMapping;
+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 << "  -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;
+	unsigned int  c=1;
+	if (!strcmp(argv[c],"-err")){
+		err=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-neff")){
+		neff=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-part")){
+		part=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-odom")){
+		odom=true;
+		c++;
+	}
+	ifstream is(argv[c]);
+	if (!is){
+		cout << "could read file "<< endl;
+		return -1;
+	}
+		c++;
+	RecordList rl;
+	rl.read(is);
+	unsigned int bestidx=rl.getBestIdx();
+	cout << endl << "best index = " <<  bestidx<< endl;
+	ofstream os(argv[c]);
+	if (! os){
+		cout << "could write file "<< endl;
+		return -1;
+	}
+	rl.printPath(os,bestidx,err,odom);
+	if(part)
+	  rl.printLastParticles(os);
+	os.close();
+	return 0;
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp

@@ -0,0 +1,40 @@
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <cstring>
+
+using namespace std;
+
+int main(int argc, char**argv){
+	if (argc<3){
+		cout << "usage gfs2neff <infilename> <nefffilename>" << endl;
+		return -1;
+	}
+	ifstream is(argv[1]);
+	if (!is){
+		cout << "could read file "<< endl;
+		return -1;
+	}
+	ofstream os(argv[2]);
+	if (! os){
+		cout << "could write file "<< endl;
+		return -1;
+	}
+	unsigned int frame=0;
+	double neff=0;
+	while(is){
+			char buf[8192];
+			is.getline(buf, 8192);
+			istringstream lineStream(buf);
+			string recordType;
+			lineStream >> recordType;
+			if (recordType=="FRAME"){
+				lineStream>> frame;
+			}
+			if (recordType=="NEFF"){
+				lineStream>> neff;
+				os << frame << " " << neff << endl;
+			}
+	}
+	os.close();
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp

@@ -0,0 +1,406 @@
+#include <cstring>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <list>
+#include <gmapping/utils/point.h>
+
+#define MAX_LINE_LENGHT (1000000)
+
+using namespace GMapping;
+using namespace std;
+
+struct Record{
+	unsigned int dim;
+	double time;
+	virtual ~Record(){}
+	virtual void read(istream& is)=0;
+	virtual void write(ostream& os){};
+};
+
+struct CommentRecord: public Record{
+	string text;
+	virtual void read(istream& is){
+		char buf[MAX_LINE_LENGHT];
+		memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
+		is.getline(buf, MAX_LINE_LENGHT);
+		text=string(buf);
+	}
+	virtual void write(ostream& os){
+		os << "#GFS_COMMENT: " << text << endl;
+	}
+};
+
+struct PoseRecord: public Record{
+	PoseRecord(bool ideal=false){
+		truePos=ideal;
+	}
+	bool truePos;
+	OrientedPoint pose;
+	void read(istream& is){
+		is >> pose.x >> pose.y >> pose.theta;
+		time = 0;
+		if (is)
+			is >> time;
+	}
+	virtual void write(ostream& os){
+		if (truePos) 
+			os << "POS-CORR";
+		else
+			os << "POS ";
+		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
+		os << "0 0: ";
+		os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta <<  endl;
+	}
+};
+
+struct NeffRecord: public Record{
+	double neff;
+	void read(istream& is){
+		is >> neff;
+	}
+	virtual void write(ostream& os){
+		os << "NEFF " << neff << endl;
+	}
+};
+
+
+struct OdometryRecord: public Record{
+	vector<OrientedPoint> poses;
+	virtual void read(istream& is){
+		is >> dim;
+		for (unsigned int i=0; i< dim; i++){
+			OrientedPoint p;
+			double w;
+			is >> p.x;
+			is >> p.y;
+			is >> p.theta;
+			is >> w;
+			poses.push_back(p);
+		}
+		time = 0;
+		if (is)
+			is >> time;
+	}
+};
+
+
+struct ScanMatchRecord: public Record{
+	vector<OrientedPoint> poses;
+	vector<double> weights;
+	virtual void read(istream& is){
+		is >> dim;
+		for (unsigned int i=0; i< dim; i++){
+			OrientedPoint p;
+			double w;
+			is >> p.x;
+			is >> p.y;
+			is >> p.theta;
+			is >> w;
+			poses.push_back(p);
+			weights.push_back(w);
+		}
+	}
+};
+
+struct LaserRecord: public Record{
+	vector<double> readings;
+	OrientedPoint pose;
+	virtual void read(istream& is){
+		is >> dim;
+		for (unsigned int i=0; i< dim; i++){
+			double r;
+			is >> r;
+			readings.push_back(r);
+		}
+		is >> pose.x;
+		is >> pose.y;
+		is >> pose.theta;
+		time = 0;
+		if (is)
+			is >> time;
+	}
+	
+	//	dummy, &sec, &usec, &nLas, &nVal, &range ) == EOF) {
+
+	virtual void write(ostream& os){
+		os << "POS ";
+		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
+		os << "0 0: ";
+		os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta <<  endl;
+		
+		os << "LASER-RANGE ";
+		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
+		os << " 0 0 0 " << dim << " 180. : ";
+		for (unsigned int i=0; i< dim; i++){
+			os <<" "<< readings[i]*100 ;
+		}
+		os << endl;
+	};
+};
+
+struct ResampleRecord: public Record{
+	vector<unsigned int> indexes;
+	virtual void read(istream& is){
+		is >> dim;
+		for (unsigned int i=0; i< dim; i++){
+			unsigned int j;
+			is >> j;
+			indexes.push_back(j);
+		}
+	}
+};
+
+struct RecordList: public list<Record*>{
+	mutable int sampleSize;
+	
+	istream& read(istream& is){
+		while(is){
+			char buf[8192];
+			is.getline(buf, 8192);
+			istringstream lineStream(buf);
+			string recordType;
+			lineStream >> recordType;
+			Record* rec=0;
+			if (recordType=="LASER_READING"){
+				rec=new LaserRecord;
+				cout << "l" << flush;
+			}
+			if (recordType=="ODO_UPDATE"){
+				rec=new OdometryRecord;
+				cout << "o" << flush;
+			}
+			if (recordType=="SM_UPDATE"){
+				rec=new ScanMatchRecord;
+				cout << "m" << flush;
+			}
+			if (recordType=="SIMULATOR_POS"){
+				rec=new PoseRecord(true);
+				cout << "t" << flush;
+			}
+			if (recordType=="RESAMPLE"){
+				rec=new ResampleRecord;
+				cout << "r" << flush;
+			}
+			if (recordType=="NEFF"){
+				rec=new NeffRecord;
+				cout << "n" << flush;
+			}
+			if (recordType=="COMMENT"){
+				rec=new CommentRecord;
+				cout << "c" << flush;
+			}
+			if (rec){
+				rec->read(lineStream);
+				push_back(rec);
+			}
+		}
+		return is;
+	}
+	
+	double getLogWeight(unsigned int i) const{
+		double weight=0;
+		unsigned int currentIndex=i;
+		for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
+			ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
+			if (scanmatch){
+				weight+=scanmatch->weights[currentIndex];
+			}
+			ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
+			if (resample){
+				currentIndex=resample->indexes[currentIndex];
+			}
+		}
+		return weight;
+	}
+	unsigned int getBestIdx() const {
+		if (empty())
+			return 0;
+		const ScanMatchRecord* scanmatch=0;
+		const_reverse_iterator it=rbegin();
+		while(!scanmatch){
+			scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+			it++;
+		}
+		unsigned int dim=scanmatch->dim;
+		sampleSize=(int)dim;
+		double bestw=-1e200;
+		unsigned int best=scanmatch->dim+1;
+		for (unsigned i=0; i<dim; i++){
+			double w=getLogWeight(i);
+			if (w>bestw){
+				best=i;
+				bestw=w;
+			}
+		}
+		return best;
+	}
+	
+	void printPath(ostream& os, unsigned int i, bool err=false) const{
+		unsigned int currentIndex=i;
+		OrientedPoint p(0,0,0);
+	
+		RecordList rl;
+		
+		//reconstruct a  path
+		for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
+			const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
+			if (neff){
+				NeffRecord* n=new NeffRecord(*neff);
+				rl.push_front(n);
+			}
+			const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+			if (scanmatch){
+				PoseRecord* pose=new PoseRecord;
+				pose->dim=0;
+				p=pose->pose=scanmatch->poses[currentIndex];
+				rl.push_front(pose);
+			}
+			const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
+			if (odometry){
+				PoseRecord* pose=new PoseRecord;
+				pose->dim=0;
+				p=pose->pose=odometry->poses[currentIndex];
+				pose->time=odometry->time;
+				rl.push_front(pose);
+			}
+			const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
+			if (tpose){
+				PoseRecord* pose=new PoseRecord(*tpose);
+				rl.push_front(pose);
+			}
+			const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
+			if (laser){
+				LaserRecord* claser=new LaserRecord(*laser);
+				claser->pose=p;
+				rl.push_front(claser);
+			}
+			const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it); 
+			if (comment){
+				CommentRecord* ccomment=new CommentRecord(*comment);
+				rl.push_front(ccomment);
+			}
+			const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
+			if (resample){
+				currentIndex=resample->indexes[currentIndex];
+				ResampleRecord* r= new ResampleRecord(*resample);
+				rl.push_front(r);
+			}
+		}
+		bool started=false;
+		double ox=0, oy=0, rxx=0, rxy=0, ryx=0, ryy=0, rth=0;
+		bool computedTransformation=false;
+		bool truePosFound=false;
+		OrientedPoint truePose(0,0,0);
+		OrientedPoint currPose(0,0,0);
+		bool tpf=false;
+		double neff=0;
+		unsigned int count=0;
+		for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
+			NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
+			if (neffr)
+				neff=neffr->neff/(double)sampleSize;
+			started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
+			if (started && ! truePosFound){
+				PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
+				if (tpose && tpose->truePos){
+					truePosFound=true;
+					tpf=true;
+					truePose=tpose->pose;
+					os << "# ";
+					(*it)->write(os);
+				}
+			}
+			if (started && truePosFound && ! computedTransformation){
+				PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+				if (pos && !pos->truePos){
+					OrientedPoint pose=pos->pose;
+					rth=truePose.theta-pose.theta;
+					double s=sin(rth), c=cos(rth);
+					rxx=ryy=c;
+					rxy=-s; ryx=s;
+					ox=truePose.x-(rxx*pose.x+rxy*pose.y);
+					oy=truePose.y-(ryx*pose.x+ryy*pose.y);
+					computedTransformation=true;
+					os << "# ";
+					(*it)->write(os);
+					
+				}
+			}
+			ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
+			if(resample){
+				os << "MARK-POS 0 0: " <<currPose.x*100 << " " << currPose.y*100 << " 0 " << count++ << endl;
+			} 
+			if (computedTransformation){
+				PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+				if (pos){
+					if (pos->truePos){
+						tpf=true;
+						truePose=pos->pose;
+					} else {
+						if (tpf){
+							tpf=false;
+							OrientedPoint pose=pos->pose;
+							double ex, ey, eth=truePose.theta-pose.theta-rth;
+							ex=truePose.x-(ox+rxx*pose.x+rxy*pose.y);
+							ey=truePose.y-(oy+ryx*pose.x+ryy*pose.y);
+							eth=atan2(sin(eth), cos(eth));
+							if (! err)
+								os << "# ERROR ";
+							os << neff << " "
+							   << ex << " " << ey << " " << eth 
+							   << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
+						}
+					}
+				}
+				
+			}
+			PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+			if (pos)
+				currPose=pos->pose;
+			
+			if (! err)
+				(*it)->write(os);
+			delete *it;
+		}
+	}
+};
+
+
+
+int main (int argc, const char * const * argv){
+	if (argc<3){
+		cout << "usage gfs2rec [-err] <infilename> <outfilename>" << endl;
+		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;
+		return -1;
+	}
+		c++;
+	RecordList rl;
+	rl.read(is);
+	unsigned int bestidx=rl.getBestIdx();
+	cout << endl << "best index = " <<  bestidx<< endl;
+	ofstream os(argv[c]);
+	if (! os){
+		cout << "could write file "<< endl;
+		return -1;
+	}
+	rl.printPath(os,bestidx,err);
+	os.close();
+	return 0;
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp

@@ -0,0 +1,58 @@
+#include <gmapping/utils/stat.h>
+#include <gmapping/particlefilter/particlefilter.h>
+#include <iostream>
+#include <fstream>
+#include "gfsreader.h"
+
+using namespace std;
+using namespace GMapping;
+using namespace GMapping::GFSReader;
+
+
+int main(int argc, char ** argv){
+	if (argc<2){
+		cout << "usage gfs2stat <infilename> <outfilename>" << endl;
+		return 0;
+	}
+	ifstream is(argv[1]);
+	if (!is){
+		cout << "no file found: " << argv[1] << endl;
+		return 0;
+	}
+	ofstream os(argv[2]);
+	if (!os){
+		cout << "cannot open file: " << argv[1] << endl;
+		return 0;
+	}
+	cout << "loading... "<< flush;
+	RecordList rl;
+	rl.read(is);
+	cout << " done" << endl;
+	int count=-1;
+	for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
+		
+		count++;
+		const ScanMatchRecord* rec=dynamic_cast<const ScanMatchRecord*>(*it);
+		if (!rec)
+			continue;
+		Gaussian3 gaussian;
+		/*	
+		vector<double> nweights;
+		cout << "N"<< flush;
+		back_insert_iterator< vector<double> > out(nweights);
+		toNormalForm(out,rec->weights.begin(), rec->weights.end());
+		cout << "G"<< flush;
+		gaussian.computeFromSamples(rec->poses, nweights);
+		*/
+		gaussian.computeFromSamples(rec->poses);
+		cout << "E"<< flush;
+		os << count <<" ";
+		os << gaussian.mean.x <<" ";
+		os << gaussian.mean.y <<" ";
+		os << gaussian.mean.theta <<" ";
+		os << gaussian.covariance.eval[0] <<" ";
+		os << gaussian.covariance.eval[1] <<" ";
+		os << gaussian.covariance.eval[2] <<endl;
+	}
+	os.close();
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfs2stream.cpp

@@ -0,0 +1,74 @@
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <list>
+#include <gmapping/utils/point.h>
+#include <gmapping/utils/commandline.h>
+#include "gfsreader.h"
+#define MAX_LINE_LENGHT (1000000)
+
+using namespace std;
+using namespace GMapping;
+using namespace GMapping::GFSReader;
+
+computeBoundingBox()
+
+
+int main (unsigned int argc, const char * const * argv)
+	double delta = 0.1;
+	double skip  = 2;
+	double rotate = 0;
+	double maxrange = 0;
+	
+	if (argc<3){
+		cout << "usage gfs2stream [-step Number]  <outfilename>" << endl;
+		return -1;
+	}
+	
+	
+	CMD_PARSE_BEGIN(1,argc-2);
+	
+	CMD_PARSE_END;
+
+	if (argc<3){
+		cout << "usage gfs2stream [-step Number]  <outfilename>" << endl;
+		return -1;
+	}
+	bool err=0;
+	bool neff=0;
+	bool part=0;
+	unsigned int  c=1;
+	if (!strcmp(argv[c],"-err")){
+		err=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-neff")){
+		neff=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-part")){
+		part=true;
+		c++;
+	}
+	ifstream is(argv[c]);
+	if (!is){
+		cout << "could read file "<< endl;
+		return -1;
+	}
+		c++;
+	RecordList rl;
+	rl.read(is);
+	unsigned int bestidx=rl.getBestIdx();
+	cout << endl << "best index = " <<  bestidx<< endl;
+	ofstream os(argv[c]);
+	if (! os){
+		cout << "could write file "<< endl;
+		return -1;
+	}
+	rl.printPath(os,bestidx,err);
+	if(part)
+		rl.printLastParticles(os);
+	os.close();
+	return 0;
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp

@@ -0,0 +1,506 @@
+#include <cstring>
+#include "gfsreader.h"
+#include <iomanip>
+#include <limits>
+
+namespace  GMapping { 
+
+namespace GFSReader{
+
+Record::~Record(){}
+void Record::write(ostream& os){};
+
+void CommentRecord::read(istream& is){
+	char buf[MAX_LINE_LENGHT];
+	memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
+	is.getline(buf, MAX_LINE_LENGHT);
+	text=string(buf);
+}
+
+void CommentRecord::write(ostream& os){
+	os << "#GFS_COMMENT: " << text << endl;
+}
+
+PoseRecord::PoseRecord(bool ideal){
+	truePos=ideal;
+}
+void PoseRecord::read(istream& is){
+	is >> pose.x >> pose.y >> pose.theta;
+	time = 0;
+	if (is)
+		is >> time;
+}
+void PoseRecord::write(ostream& os){
+	if (truePos)
+		os << "TRUEPOS ";
+	else
+		os << "ODOM ";
+	os << setiosflags(ios::fixed) << setprecision(6);
+	os << pose.x << " " << pose.y << " " << pose.theta << " 0 0 0 ";
+	os << time << " pippo " << time << endl;
+}
+
+void NeffRecord::read(istream& is){
+        is >> neff;
+        time =0;
+	if (is)
+	  is >> time;
+
+}
+
+void NeffRecord::write(ostream& os){
+	os << "NEFF " << neff ;
+	os << setiosflags(ios::fixed) << setprecision(6);
+	os << " " << time << " pippo " << time << endl;
+}
+
+
+void OdometryRecord::read(istream& is){
+	is >> dim;
+	for (unsigned int i=0; i< dim; i++){
+		OrientedPoint p;
+		double w;
+		is >> p.x;
+		is >> p.y;
+		is >> p.theta;
+		is >> w;
+		poses.push_back(p);
+	}
+	time = 0;
+	if (is)
+		is >> time;
+}
+
+void RawOdometryRecord::read(istream& is){
+  is >> pose.x;
+  is >> pose.y;
+  is >> pose.theta;
+  time = 0;
+  assert(is);
+    is >> time;
+ 
+}
+
+
+void EntropyRecord::read(istream& is){
+  is >> poseEntropy >> trajectoryEntropy >> mapEntropy;
+  time =0;
+  if (is)
+    is >> time;
+}
+
+void EntropyRecord::write(ostream& os){
+  os << setiosflags(ios::fixed) << setprecision(6) << "ENTROPY " << poseEntropy << " " << trajectoryEntropy << " " << mapEntropy;
+  os << " " << time << " pippo " << time << endl;
+}
+
+void ScanMatchRecord::read(istream& is){
+	is >> dim;
+	for (unsigned int i=0; i< dim; i++){
+		OrientedPoint p;
+		double w;
+		is >> p.x;
+		is >> p.y;
+		is >> p.theta;
+		is >> w;
+		poses.push_back(p);
+		weights.push_back(w);
+	}
+}
+
+void LaserRecord::read(istream& is){
+	is >> dim;
+	for (unsigned int i=0; i< dim; i++){
+		double r;
+		is >> r;
+		readings.push_back(r);
+	}
+	is >> pose.x;
+	is >> pose.y;
+	is >> pose.theta;
+	time = 0;
+	if (is)
+		is >> time;
+}
+
+void LaserRecord::write(ostream& os){
+	os << "WEIGHT " << weight << endl;
+	os << "ROBOTLASER1 ";
+	
+        
+        if ((dim == 541)||(dim == 540)) { // S300
+          os <<" 4";  // laser type
+          os <<" -2.351831";  // start_angle
+          os <<" 4.712389";  // fov
+          os <<" 0.008727";  // angular res
+          os <<" 30.0" ;  // maxrange
+        }
+        else if ((dim == 180)||(dim == 181)) { // PLS
+          os <<" 0";  // laser type
+          os <<" -1.570796";  // start_angle
+          os <<" 3.141593";  // fov
+          os <<" 0.017453";  // angular res
+          os <<" 81.9" ;  // maxrange
+        }
+        else if ((dim == 360)||(dim == 361)) { // LMS
+          os <<" 0";  // laser type
+          os <<" -1.570796";  // start_angle
+          os <<" 3.141593";  // fov
+          os <<" 0.008726";  // angular res
+          os <<" 81.9" ;  // maxrange
+        }
+        else if ((dim == 682)||(dim == 683)) { // URG
+          os <<" 0";  // laser type
+          os <<" -2.094395";  // start_angle
+          os <<" 4.1887902";  // fov
+          os << " " <<   360.0/1024.0/180.0*M_PI;  // angular res
+          os <<" 5.5" ;  // maxrange
+        }
+        else {     // PLS
+          os <<" 0";  // laser type
+          os <<" -1.570796";  // start_angle
+          os <<" 3.141593";  // fov
+          os <<" 0.017453";  // angular res
+          os <<" 81.9" ;  // maxrange
+        }
+	os <<" 0.01"; // accuracy	
+	os <<" 0" ;  // remission mode
+	os <<" "<< dim; // num readings
+        os << setiosflags(ios::fixed) << setprecision(2);
+	for (unsigned int i=0; i< dim; i++){
+		os <<" "<< readings[i] ;
+	}
+	os << setiosflags(ios::fixed) << setprecision(6);
+	os <<" 0"; // num remession values
+	os <<" "<< pose.x;
+	os <<" "<< pose.y;
+	os <<" "<< pose.theta;
+	os <<" "<< pose.x;
+	os <<" "<< pose.y;
+	os <<" "<< pose.theta;
+	os <<" 0" ;  // tv
+	os <<" 0" ;  // rv
+	os <<" 0.55" ;  // forward_safety_dist
+	os <<" 0.375" ;     // sideward_safety_dist
+	os <<" 1000000.0" ; // turn_axis
+	os <<" "<< time <<  " localhost " << time << endl;
+};
+
+void ResampleRecord::read(istream& is){
+	is >> dim;
+	for (unsigned int i=0; i< dim; i++){
+		unsigned int j;
+		is >> j;
+		indexes.push_back(j);
+	}
+}
+
+istream& RecordList::read(istream& is){
+	while(is){
+		char buf[MAX_LINE_LENGHT];
+		is.getline(buf, MAX_LINE_LENGHT);
+		istringstream lineStream(buf);
+		string recordType;
+		lineStream >> recordType;
+		Record* rec=0;
+		if (recordType=="LASER_READING"){
+			rec=new LaserRecord;
+//			cout << "l" << flush;
+		}
+		else if (recordType=="ODO_UPDATE"){
+			rec=new OdometryRecord;
+//			cout << "o" << flush;
+		}
+		else if (recordType=="ODOM"){
+			rec=new RawOdometryRecord;
+//			cout << "O" << flush;
+		}
+		else if (recordType=="SM_UPDATE"){
+			rec=new ScanMatchRecord;
+//			cout << "m" << flush;
+		}
+		else if (recordType=="SIMULATOR_POS"){
+			rec=new PoseRecord(true);
+//			cout << "t" << flush;
+		}
+		else if (recordType=="RESAMPLE"){
+			rec=new ResampleRecord;
+//			cout << "r" << flush;
+		}
+		else if (recordType=="NEFF"){
+			rec=new NeffRecord;
+//			cout << "n" << flush;
+		}
+		else if (recordType=="COMMENT" || recordType=="#COMMENT"){
+			rec=new CommentRecord;
+//			cout << "c" << flush;
+		}
+		else if (recordType=="ENTROPY"){
+			rec=new EntropyRecord;
+//			cout << "c" << flush;
+		}
+		
+		if (rec){
+			rec->read(lineStream);
+			push_back(rec);
+		}
+	}
+	return is;
+}
+
+double RecordList::getLogWeight(unsigned int i) const{
+	double weight=0;
+	unsigned int currentIndex=i;
+	for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
+		ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
+		if (scanmatch){
+			weight+=scanmatch->weights[currentIndex];
+		}
+		ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
+		if (resample){
+			currentIndex=resample->indexes[currentIndex];
+		}
+	}
+	return weight;
+}
+
+double RecordList::getLogWeight(unsigned int i, RecordList::const_iterator frame) const{
+	double weight=0;
+	unsigned int currentIndex=i;
+	for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){
+		ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
+		if (scanmatch){
+			weight+=scanmatch->weights[currentIndex];
+		}
+		ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
+		if (resample){
+			currentIndex=resample->indexes[currentIndex];
+		}
+	}
+	return weight;
+}
+
+unsigned int RecordList::getBestIdx() const {
+	if (empty())
+		return 0;
+	const ScanMatchRecord* scanmatch=0;
+	const_reverse_iterator it=rbegin();
+	while(!scanmatch){
+		scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+		it++;
+	}
+	unsigned int dim=scanmatch->dim;
+	sampleSize=(int)dim;
+	double bestw=-std::numeric_limits<double>::max();
+	unsigned int best=scanmatch->dim+1;
+	for (unsigned i=0; i<dim; i++){
+		double w=getLogWeight(i);
+		if (w>bestw){
+			best=i;
+			bestw=w;
+		}
+	}
+	return best;
+}
+
+void RecordList::printLastParticles(ostream& os) const {
+	if (empty())
+		return;
+	const ScanMatchRecord* scanmatch=0;
+	const_reverse_iterator it=rbegin();
+	while(!scanmatch){
+		scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+		it++;
+	}
+	if (! scanmatch)
+		return;
+	for (vector<OrientedPoint>::const_iterator it=scanmatch->poses.begin(); it!=scanmatch->poses.end(); it++){
+		os << "MARKER [color=black; circle=" << it->x*100 << "," << it->y*100 << ",10] 0 pippo 0" << endl;
+	}
+}
+
+void RecordList::destroyReferences(){
+	for(RecordList::iterator it=begin(); it!=end(); it++)
+		delete (*it);
+	
+}
+
+RecordList RecordList::computePath(unsigned int i, RecordList::const_iterator frame) const{
+	unsigned int currentIndex=i;
+	OrientedPoint p(0,0,0);
+	RecordList rl;
+	
+	//reconstruct a  path
+	bool first=true;
+	for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){
+		const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+		if (scanmatch){
+			p=scanmatch->poses[currentIndex];
+			first=false;
+		}
+		const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
+		if (laser && !first){
+			LaserRecord* claser=new LaserRecord(*laser);
+			claser->pose=p;
+			rl.push_front(claser);
+		}
+		const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
+		if (resample){
+			currentIndex=resample->indexes[currentIndex];
+		}
+	}
+	return rl;
+}
+
+	
+void RecordList::printPath(ostream& os, unsigned int i, bool err, bool rawodom) const{
+	unsigned int currentIndex=i;
+	OrientedPoint p(0,0,0);
+	RecordList rl;
+	double oldWeight=0;
+	double w=0;
+	//reconstruct a  path
+	for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
+		const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
+		if (neff){
+			NeffRecord* n=new NeffRecord(*neff);
+			rl.push_front(n);
+		}
+		const EntropyRecord* entropy=dynamic_cast<const EntropyRecord*>(*it);
+		if (entropy){
+		    EntropyRecord* n=new EntropyRecord(*entropy);
+		    rl.push_front(n);
+		}
+		const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+		if (scanmatch){
+			PoseRecord* pose=new PoseRecord;
+			pose->dim=0;
+			p=pose->pose=scanmatch->poses[currentIndex];
+			w=scanmatch->weights[currentIndex]-oldWeight;
+			oldWeight=scanmatch->weights[currentIndex];
+			
+			if (!rawodom) {
+			  rl.push_front(pose);
+			}
+		}
+		const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
+		if (odometry){
+		  PoseRecord* pose=new PoseRecord;
+		  pose->dim=0;
+		  p=pose->pose=odometry->poses[currentIndex];
+		  pose->time=odometry->time;
+		  if (!rawodom) {
+		    rl.push_front(pose);
+		  }
+		}
+		const RawOdometryRecord* rawodometry=dynamic_cast<const RawOdometryRecord*>(*it);
+		if (rawodometry){
+		  PoseRecord* pose=new PoseRecord;
+		  pose->dim=0;
+		  pose->pose=rawodometry->pose;
+		  pose->time=rawodometry->time;
+		  if (rawodom) {
+		    rl.push_front(pose);
+		  }
+ 		}
+		const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
+		if (tpose){
+			PoseRecord* pose=new PoseRecord(*tpose);
+			rl.push_front(pose);
+		}
+		const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
+		if (laser){
+			LaserRecord* claser=new LaserRecord(*laser);
+			claser->pose=p;
+			claser->weight=w;
+			rl.push_front(claser);
+		}
+		const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it); 
+		if (comment){
+			CommentRecord* ccomment=new CommentRecord(*comment);
+			rl.push_front(ccomment);
+		}
+		const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
+		if (resample){
+			rl.push_front(new ResampleRecord(*resample));
+			currentIndex=resample->indexes[currentIndex];
+		}
+		
+	}
+	bool started=false;
+	bool computedTransformation=false;
+	bool truePosFound=false;
+	OrientedPoint truePose;
+	OrientedPoint oldPose;
+	OrientedPoint trueStart, realStart;
+	bool tpf=false;
+	double neff=0;
+	double totalError=0;
+	int count=0;
+	for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
+		NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
+		if (neffr)
+			neff=neffr->neff/(double)sampleSize;
+		started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
+		if (started && ! truePosFound){
+			PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
+			if (tpose && tpose->truePos){
+				truePosFound=true;
+				tpf=true;
+				truePose=tpose->pose;
+				os << "# ";
+				(*it)->write(os);
+			}
+		}
+		if (started && truePosFound && ! computedTransformation){
+			PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+			if (pos && !pos->truePos){
+				trueStart=truePose;
+				realStart=pos->pose;
+				os << "# ";
+				(*it)->write(os);
+				computedTransformation=true;
+			}
+		}
+		if (computedTransformation){
+		  os << setiosflags(ios::fixed) << setprecision(6);
+			PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+			if (pos){
+				if (pos->truePos){
+					tpf=true;
+					truePose=pos->pose;
+				} else {
+					if (tpf){
+						tpf=false;
+						OrientedPoint realDelta=absoluteDifference(pos->pose,realStart);
+						OrientedPoint trueDelta=absoluteDifference(truePose,trueStart);
+						double ex=realDelta.x-trueDelta.x;
+						double ey=realDelta.y-trueDelta.y;
+						double eth=realDelta.theta-trueDelta.theta;
+						eth=atan2(sin(eth), cos(eth));
+						if (! err)
+							os << "# ERROR ";
+						os << neff << " "
+							<< ex << " " << ey << " " << eth 
+							<< " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
+						totalError+=sqrt(ex*ex+ey*ey);
+						count++;
+					}
+				}
+			}
+			
+		}
+		PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+		if (pos)
+			oldPose=pos->pose;
+		if (! err)
+			(*it)->write(os);
+		delete *it;
+	}
+	if (err)
+		cout << "average error" << totalError/count << endl;
+}
+
+}; //gfsreader
+
+}; //GMapping;

slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h

@@ -0,0 +1,101 @@
+#ifndef GFSREADER_H
+#define GFSREADER_H
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <list>
+#include <gmapping/utils/point.h>
+
+#define MAX_LINE_LENGHT (1000000)
+
+namespace GMapping{
+
+namespace GFSReader{
+	
+using namespace std;
+
+struct Record{
+	unsigned int dim;
+	double time;
+	virtual ~Record();
+	virtual void read(istream& is)=0;
+	virtual void write(ostream& os);
+};
+
+struct CommentRecord: public Record{
+	string text;
+	virtual void read(istream& is);
+	virtual void write(ostream& os);
+};
+
+struct PoseRecord: public Record{
+	PoseRecord(bool ideal=false);
+	void read(istream& is);
+	virtual void write(ostream& os);
+	bool truePos;
+	OrientedPoint pose;
+};
+
+struct NeffRecord: public Record{
+	void read(istream& is);
+	virtual void write(ostream& os);
+	double neff;
+};
+
+struct EntropyRecord: public Record{
+	void read(istream& is);
+	virtual void write(ostream& os);
+	double poseEntropy;
+	double trajectoryEntropy;
+	double mapEntropy;
+};
+
+
+struct OdometryRecord: public Record{
+	virtual void read(istream& is);
+	vector<OrientedPoint> poses;
+};
+
+struct RawOdometryRecord: public Record{
+	virtual void read(istream& is);
+	OrientedPoint pose;
+};
+
+struct ScanMatchRecord: public Record{
+	virtual void read(istream& is);
+	vector<OrientedPoint> poses;
+	vector<double> weights;
+};
+
+struct LaserRecord: public Record{
+	virtual void read(istream& is);
+	virtual void write(ostream& os);
+	vector<double> readings;
+	OrientedPoint pose;
+	double weight;
+};
+
+struct ResampleRecord: public Record{
+	virtual void read(istream& is);
+	vector<unsigned int> indexes;
+};
+
+struct RecordList: public list<Record*>{
+	mutable int sampleSize;
+	istream& read(istream& is);
+	double getLogWeight(unsigned int i) const;
+	double getLogWeight(unsigned int i, RecordList::const_iterator frame) const;
+	unsigned int getBestIdx() const ;
+	void printLastParticles(ostream& os) const ;
+	void printPath(ostream& os, unsigned int i, bool err=false, bool rawodom=false) const;
+	RecordList computePath(unsigned int i, RecordList::const_iterator frame) const;
+	void destroyReferences();
+};
+
+}; //end namespace GFSReader
+
+}; //end namespace GMapping
+
+#endif

slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp

@@ -0,0 +1,518 @@
+#include <string>
+#include <deque>
+#include <list>
+#include <map>
+#include <set>
+#include <fstream>
+#include <iomanip>
+#include <gmapping/utils/stat.h>
+#include <gmapping/gridfastslam/gridslamprocessor.h>
+
+//#define MAP_CONSISTENCY_CHECK
+//#define GENERATE_TRAJECTORIES
+
+namespace GMapping {
+
+const double m_distanceThresholdCheck = 20;
+ 
+using namespace std;
+
+  GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){
+    
+    period_ = 5.0;
+    m_obsSigmaGain=1;
+    m_resampleThreshold=0.5;
+    m_minimumScore=0.;
+  }
+  
+  GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) 
+    :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout){
+    
+    period_ = 5.0;
+
+    m_obsSigmaGain=gsp.m_obsSigmaGain;
+    m_resampleThreshold=gsp.m_resampleThreshold;
+    m_minimumScore=gsp.m_minimumScore;
+    
+    m_beams=gsp.m_beams;
+    m_indexes=gsp.m_indexes;
+    m_motionModel=gsp.m_motionModel;
+    m_resampleThreshold=gsp.m_resampleThreshold;
+    m_matcher=gsp.m_matcher;
+    
+    m_count=gsp.m_count;
+    m_readingCount=gsp.m_readingCount;
+    m_lastPartPose=gsp.m_lastPartPose;
+    m_pose=gsp.m_pose;
+    m_odoPose=gsp.m_odoPose;
+    m_linearDistance=gsp.m_linearDistance;
+    m_angularDistance=gsp.m_angularDistance;
+    m_neff=gsp.m_neff;
+	
+    cerr << "FILTER COPY CONSTRUCTOR" << endl;
+    cerr << "m_odoPose=" << m_odoPose.x << " " <<m_odoPose.y << " " << m_odoPose.theta << endl;
+    cerr << "m_lastPartPose=" << m_lastPartPose.x << " " <<m_lastPartPose.y << " " << m_lastPartPose.theta << endl;
+    cerr << "m_linearDistance=" << m_linearDistance << endl;
+    cerr << "m_angularDistance=" << m_linearDistance << endl;
+    
+		
+    m_xmin=gsp.m_xmin;
+    m_ymin=gsp.m_ymin;
+    m_xmax=gsp.m_xmax;
+    m_ymax=gsp.m_ymax;
+    m_delta=gsp.m_delta;
+    
+    m_regScore=gsp.m_regScore;
+    m_critScore=gsp.m_critScore;
+    m_maxMove=gsp.m_maxMove;
+    
+    m_linearThresholdDistance=gsp.m_linearThresholdDistance;
+    m_angularThresholdDistance=gsp.m_angularThresholdDistance;
+    m_obsSigmaGain=gsp.m_obsSigmaGain;
+    
+#ifdef MAP_CONSISTENCY_CHECK
+    cerr << __PRETTY_FUNCTION__ <<  ": trajectories copy.... ";
+#endif
+    TNodeVector v=gsp.getTrajectories();
+    for (unsigned int i=0; i<v.size(); i++){
+		m_particles[i].node=v[i];
+    }
+#ifdef MAP_CONSISTENCY_CHECK
+    cerr <<  "end" << endl;
+#endif
+
+
+    cerr  << "Tree: normalizing, resetting and propagating weights within copy construction/cloneing ..." ;
+    updateTreeWeights(false);
+    cerr  << ".done!" <<endl;
+  }
+  
+  GridSlamProcessor::GridSlamProcessor(std::ostream& infoS): m_infoStream(infoS){
+    period_ = 5.0;
+    m_obsSigmaGain=1;
+    m_resampleThreshold=0.5;
+    m_minimumScore=0.;
+	
+  }
+
+  GridSlamProcessor* GridSlamProcessor::clone() const {
+# ifdef MAP_CONSISTENCY_CHECK
+    cerr << __PRETTY_FUNCTION__ << ": performing preclone_fit_test" << endl;
+    typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
+    PointerMap pmap;
+	for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	  const ScanMatcherMap& m1(it->map);
+	  const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
+ 	  for (int x=0; x<h1.getXSize(); x++){
+	    for (int y=0; y<h1.getYSize(); y++){
+	      const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
+	      if (a1.m_reference){
+		PointerMap::iterator f=pmap.find(a1.m_reference);
+		if (f==pmap.end())
+		  pmap.insert(make_pair(a1.m_reference, 1));
+		else
+		  f->second++;
+	      }
+	    }
+	  }
+	}
+	cerr << __PRETTY_FUNCTION__ <<  ": Number of allocated chunks" << pmap.size() << endl;
+	for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++)
+	  assert(it->first->shares==(unsigned int)it->second);
+
+	cerr << __PRETTY_FUNCTION__ <<  ": SUCCESS, the error is somewhere else" << endl;
+# endif
+	GridSlamProcessor* cloned=new GridSlamProcessor(*this);
+	
+# ifdef MAP_CONSISTENCY_CHECK
+	cerr << __PRETTY_FUNCTION__ <<  ": trajectories end" << endl;
+	cerr << __PRETTY_FUNCTION__ << ": performing afterclone_fit_test" << endl;
+	ParticleVector::const_iterator jt=cloned->m_particles.begin();
+	for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	  const ScanMatcherMap& m1(it->map);
+	  const ScanMatcherMap& m2(jt->map);
+	  const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
+	  const HierarchicalArray2D<PointAccumulator>& h2(m2.storage());
+	  jt++;
+ 	  for (int x=0; x<h1.getXSize(); x++){
+	    for (int y=0; y<h1.getYSize(); y++){
+	      const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
+	      const autoptr< Array2D<PointAccumulator> >& a2(h2.m_cells[x][y]);
+	      assert(a1.m_reference==a2.m_reference);
+	      assert((!a1.m_reference) || !(a1.m_reference->shares%2));
+	    }
+	  }
+	}
+	cerr << __PRETTY_FUNCTION__ <<  ": SUCCESS, the error is somewhere else" << endl;
+# endif
+	return cloned;
+}
+  
+  GridSlamProcessor::~GridSlamProcessor(){
+    cerr << __PRETTY_FUNCTION__ << ": Start" << endl;
+    cerr << __PRETTY_FUNCTION__ << ": Deleting tree" << endl;
+    for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+#ifdef TREE_CONSISTENCY_CHECK		
+      TNode* node=it->node;
+      while(node)
+	node=node->parent;
+      cerr << "@" << endl;
+#endif
+      if (it->node)
+	delete it->node;
+      //cout << "l=" << it->weight<< endl;
+    }
+    
+# ifdef MAP_CONSISTENCY_CHECK
+    cerr << __PRETTY_FUNCTION__ << ": performing predestruction_fit_test" << endl;
+    typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
+    PointerMap pmap;
+    for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+      const ScanMatcherMap& m1(it->map);
+      const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
+      for (int x=0; x<h1.getXSize(); x++){
+	for (int y=0; y<h1.getYSize(); y++){
+	  const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
+	  if (a1.m_reference){
+	    PointerMap::iterator f=pmap.find(a1.m_reference);
+	    if (f==pmap.end())
+	      pmap.insert(make_pair(a1.m_reference, 1));
+	    else
+	      f->second++;
+	  }
+	}
+      }
+    }
+    cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl;
+    for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++)
+      assert(it->first->shares>=(unsigned int)it->second);
+    cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl;
+# endif
+  }
+
+
+		
+  void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, 
+						 int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){
+    m_obsSigmaGain=likelihoodGain;
+    m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip);
+    if (m_infoStream)
+      m_infoStream << " -maxUrange "<< urange
+		   << " -maxUrange "<< range
+		   << " -sigma     "<< sigma
+		   << " -kernelSize "<< kernsize
+		   << " -lstep "    << lopt
+		   << " -lobsGain " << m_obsSigmaGain
+		   << " -astep "    << aopt << endl;
+    
+    
+  }
+  
+void GridSlamProcessor::setMotionModelParameters
+(double srr, double srt, double str, double stt){
+  m_motionModel.srr=srr;
+  m_motionModel.srt=srt;
+  m_motionModel.str=str;
+  m_motionModel.stt=stt;	
+  
+  if (m_infoStream)
+    m_infoStream << " -srr "<< srr 	<< " -srt "<< srt  
+		 << " -str "<< str 	<< " -stt "<< stt << endl;
+  
+}
+  
+  void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){
+    m_linearThresholdDistance=linear; 
+    m_angularThresholdDistance=angular;
+    m_resampleThreshold=resampleThreshold;	
+    if (m_infoStream)
+      m_infoStream << " -linearUpdate " << linear
+		   << " -angularUpdate "<< angular
+		   << " -resampleThreshold " << m_resampleThreshold << endl;
+  }
+  
+  //HERE STARTS THE BEEF
+
+  GridSlamProcessor::Particle::Particle(const ScanMatcherMap& m):
+    map(m), pose(0,0,0), weight(0), weightSum(0), gweight(0), previousIndex(0){
+    node=0;
+  }
+  
+  
+  void GridSlamProcessor::setSensorMap(const SensorMap& smap){
+    
+    /*
+      Construct the angle table for the sensor
+      
+      FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot 
+    */
+    
+    SensorMap::const_iterator laser_it=smap.find(std::string("FLASER"));
+    if (laser_it==smap.end()){
+      cerr << "Attempting to load the new carmen log format" << endl;
+      laser_it=smap.find(std::string("ROBOTLASER1"));
+      assert(laser_it!=smap.end());
+    }
+    const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
+    assert(rangeSensor && rangeSensor->beams().size());
+    
+    m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
+    double* angles=new double[rangeSensor->beams().size()];
+    for (unsigned int i=0; i<m_beams; i++){
+      angles[i]=rangeSensor->beams()[i].pose.theta;
+    }
+    m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
+    delete [] angles;
+  }
+  
+  void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){
+    m_xmin=xmin;
+    m_ymin=ymin;
+    m_xmax=xmax;
+    m_ymax=ymax;
+    m_delta=delta;
+    if (m_infoStream)
+      m_infoStream 
+	<< " -xmin "<< m_xmin
+	<< " -xmax "<< m_xmax
+	<< " -ymin "<< m_ymin
+	<< " -ymax "<< m_ymax
+	<< " -delta "<< m_delta
+	<< " -particles "<< size << endl;
+    
+
+    m_particles.clear();
+    TNode* node=new TNode(initialPose, 0, 0, 0);
+    ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
+    for (unsigned int i=0; i<size; i++){
+      m_particles.push_back(Particle(lmap));
+      m_particles.back().pose=initialPose;
+      m_particles.back().previousPose=initialPose;
+      m_particles.back().setWeight(0);
+      m_particles.back().previousIndex=0;
+      
+		// this is not needed
+		//		m_particles.back().node=new TNode(initialPose, 0, node, 0);
+
+		// we use the root directly
+		m_particles.back().node= node;
+    }
+    m_neff=(double)size;
+    m_count=0;
+    m_readingCount=0;
+    m_linearDistance=m_angularDistance=0;
+  }
+
+  void GridSlamProcessor::processTruePos(const OdometryReading& o){
+    const OdometrySensor* os=dynamic_cast<const OdometrySensor*>(o.getSensor());
+    if (os && os->isIdeal() && m_outputStream){
+      m_outputStream << setiosflags(ios::fixed) << setprecision(3);
+      m_outputStream <<  "SIMULATOR_POS " <<  o.getPose().x << " " << o.getPose().y << " " ;
+      m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " <<  o.getTime() << endl;
+    }
+  }
+  
+  
+  bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
+    
+    /**retireve the position from the reading, and compute the odometry*/
+    OrientedPoint relPose=reading.getPose();
+    if (!m_count){
+      m_lastPartPose=m_odoPose=relPose;
+    }
+    
+    //write the state of the reading and update all the particles using the motion model
+    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+      OrientedPoint& pose(it->pose);
+      pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
+    }
+
+    // update the output file
+    if (m_outputStream.is_open()){
+      m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+      m_outputStream << "ODOM ";
+      m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
+      m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
+      m_outputStream << reading.getTime();
+      m_outputStream << endl;
+    }
+    if (m_outputStream.is_open()){
+      m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+      m_outputStream << "ODO_UPDATE "<< m_particles.size() << " ";
+      for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	OrientedPoint& pose(it->pose);
+	m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
+	m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
+      }
+      m_outputStream << reading.getTime();
+      m_outputStream << endl;
+    }
+    
+    //invoke the callback
+    onOdometryUpdate();
+    
+
+    // accumulate the robot translation and rotation
+    OrientedPoint move=relPose-m_odoPose;
+    move.theta=atan2(sin(move.theta), cos(move.theta));
+    m_linearDistance+=sqrt(move*move);
+    m_angularDistance+=fabs(move.theta);
+    
+    // if the robot jumps throw a warning
+    if (m_linearDistance>m_distanceThresholdCheck){
+      cerr << "***********************************************************************" << endl;
+      cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
+      cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;
+      cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y 
+	   << " " <<m_odoPose.theta << endl;
+      cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y 
+	   << " " <<relPose.theta << endl;
+      cerr << "***********************************************************************" << endl;
+      cerr << "** The Odometry has a big jump here. This is probably a bug in the   **" << endl;
+      cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
+      cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
+      cerr << "***********************************************************************" << endl;
+    }
+    
+    m_odoPose=relPose;
+    
+    bool processed=false;
+
+    // process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed
+    if (! m_count 
+	|| m_linearDistance>=m_linearThresholdDistance 
+	|| m_angularDistance>=m_angularThresholdDistance
+    || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){
+      last_update_time_ = reading.getTime();      
+
+      if (m_outputStream.is_open()){
+	m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+	m_outputStream << "FRAME " <<  m_readingCount;
+	m_outputStream << " " << m_linearDistance;
+	m_outputStream << " " << m_angularDistance << endl;
+      }
+      
+      if (m_infoStream)
+	m_infoStream << "update frame " <<  m_readingCount << endl
+		     << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
+      
+      
+      cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y 
+	   << " " << reading.getPose().theta << endl;
+      
+      
+      //this is for converting the reading in a scan-matcher feedable form
+      assert(reading.size()==m_beams);
+      double * plainReading = new double[m_beams];
+      for(unsigned int i=0; i<m_beams; i++){
+	plainReading[i]=reading[i];
+      }
+      m_infoStream << "m_count " << m_count << endl;
+
+      RangeReading* reading_copy = 
+              new RangeReading(reading.size(),
+                               &(reading[0]),
+                               static_cast<const RangeSensor*>(reading.getSensor()),
+                               reading.getTime());
+
+      if (m_count>0){
+	scanMatch(plainReading);
+	if (m_outputStream.is_open()){
+	  m_outputStream << "LASER_READING "<< reading.size() << " ";
+	  m_outputStream << setiosflags(ios::fixed) << setprecision(2);
+	  for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){
+	    m_outputStream << *b << " ";
+	  }
+	  OrientedPoint p=reading.getPose();
+	  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+	  m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl;
+	  m_outputStream << "SM_UPDATE "<< m_particles.size() << " ";
+	  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	    const OrientedPoint& pose=it->pose;
+	    m_outputStream << setiosflags(ios::fixed) << setprecision(3) <<  pose.x << " " << pose.y << " ";
+	    m_outputStream << setiosflags(ios::fixed) << setprecision(6) <<  pose.theta << " " << it-> weight << " ";
+	  }
+	  m_outputStream << endl;
+	}
+	onScanmatchUpdate();
+	
+	updateTreeWeights(false);
+				
+	if (m_infoStream){
+	  m_infoStream << "neff= " << m_neff  << endl;
+	}
+	if (m_outputStream.is_open()){
+	  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+	  m_outputStream << "NEFF " << m_neff << endl;
+	}
+ 	resample(plainReading, adaptParticles, reading_copy);
+	
+      } else {
+	m_infoStream << "Registering First Scan"<< endl;
+	for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){	
+	  m_matcher.invalidateActiveArea();
+	  m_matcher.computeActiveArea(it->map, it->pose, plainReading);
+	  m_matcher.registerScan(it->map, it->pose, plainReading);
+	  
+	  // cyr: not needed anymore, particles refer to the root in the beginning!
+	  TNode* node=new	TNode(it->pose, 0., it->node,  0);
+	  //node->reading=0;
+      node->reading = reading_copy;
+	  it->node=node;
+	  
+	}
+      }
+      //		cerr  << "Tree: normalizing, resetting and propagating weights at the end..." ;
+      updateTreeWeights(false);
+      //		cerr  << ".done!" <<endl;
+      
+      delete [] plainReading;
+      m_lastPartPose=m_odoPose; //update the past pose for the next iteration
+      m_linearDistance=0;
+      m_angularDistance=0;
+      m_count++;
+      processed=true;
+      
+      //keep ready for the next step
+      for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	it->previousPose=it->pose;
+      }
+      
+    }
+    if (m_outputStream.is_open())
+      m_outputStream << flush;
+    m_readingCount++;
+    return processed;
+  }
+  
+  
+  std::ofstream& GridSlamProcessor::outputStream(){
+    return m_outputStream;
+  }
+  
+  std::ostream& GridSlamProcessor::infoStream(){
+    return m_infoStream;
+  }
+  
+  
+  int GridSlamProcessor::getBestParticleIndex() const{
+    unsigned int bi=0;
+    double bw=-std::numeric_limits<double>::max();
+    for (unsigned int i=0; i<m_particles.size(); i++)
+      if (bw<m_particles[i].weightSum){
+	bw=m_particles[i].weightSum;
+	bi=i;
+      }
+    return (int) bi;
+  }
+
+  void GridSlamProcessor::onScanmatchUpdate(){}
+  void GridSlamProcessor::onResampleUpdate(){}
+  void GridSlamProcessor::onOdometryUpdate(){}
+
+  
+};// end namespace
+
+
+
+

slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor_tree.cpp

@@ -0,0 +1,263 @@
+#include <string>
+#include <deque>
+#include <list>
+#include <map>
+#include <set>
+#include <fstream>
+//#include <gsl/gsl_blas.h>
+
+#include <gmapping/utils/stat.h>
+#include <gmapping/gridfastslam/gridslamprocessor.h>
+
+namespace GMapping {
+
+using namespace std;
+
+GridSlamProcessor::TNode::TNode(const OrientedPoint& p, double w, TNode* n, unsigned int c){
+	pose=p;
+	weight=w;
+	childs=c;
+	parent=n;
+	reading=0;
+	gweight=0;
+	if (n){
+		n->childs++;
+	}
+	flag=0;
+	accWeight=0;
+}
+
+
+GridSlamProcessor::TNode::~TNode(){
+	if (parent && (--parent->childs)<=0)
+		delete parent;
+	assert(!childs);
+}
+
+
+//BEGIN State Save/Restore
+
+GridSlamProcessor::TNodeVector GridSlamProcessor::getTrajectories() const{
+  TNodeVector v;
+  TNodeMultimap parentCache;
+  TNodeDeque border;
+	
+	for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+		TNode* node=it->node;
+		while(node){
+			node->flag=false;
+			node=node->parent;
+		}
+	}
+	
+	for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+		TNode* newnode=new TNode(* (it->node) );
+		
+		v.push_back(newnode);
+		assert(newnode->childs==0);
+		if (newnode->parent){
+			parentCache.insert(make_pair(newnode->parent, newnode));
+			//cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl;
+			if (! newnode->parent->flag){
+				//cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl;
+				newnode->parent->flag=true;
+				border.push_back(newnode->parent);
+			}
+		}
+	}
+	
+	//cerr << __PRETTY_FUNCTION__ << ": border.size(INITIAL)=" << border.size() << endl;
+	//cerr << __PRETTY_FUNCTION__ << ": parentCache.size()=" << parentCache.size() << endl;
+	while (! border.empty()){
+		//cerr << __PRETTY_FUNCTION__ << ": border.size(PREPROCESS)=" << border.size() << endl;
+		//cerr << __PRETTY_FUNCTION__ << ": parentCache.size(PREPROCESS)=" << parentCache.size() << endl;
+		const TNode* node=border.front();
+		//cerr << __PRETTY_FUNCTION__ << ": node " << node << endl;
+		border.pop_front();
+		if (! node)
+			continue;
+			
+		TNode* newnode=new TNode(*node);
+		node->flag=false;
+		
+		//update the parent of all of the referring childs 
+		pair<TNodeMultimap::iterator, TNodeMultimap::iterator> p=parentCache.equal_range(node);
+		double childs=0;
+		for (TNodeMultimap::iterator it=p.first; it!=p.second; it++){
+			assert(it->second->parent==it->first);
+			(it->second)->parent=newnode;
+			//cerr << "PS(" << it->first << ", "<< it->second << ")";
+			childs++;
+		}
+		////cerr << endl;
+		parentCache.erase(p.first, p.second);
+		//cerr << __PRETTY_FUNCTION__ << ": parentCache.size(POSTERASE)=" << parentCache.size() << endl;
+		assert(childs==newnode->childs);
+		
+		//unmark the node
+		if ( node->parent ){
+			parentCache.insert(make_pair(node->parent, newnode));
+			if(! node->parent->flag){
+				border.push_back(node->parent);
+				node->parent->flag=true;
+			}	
+		}
+		//insert the parent in the cache
+	}
+	//cerr << __PRETTY_FUNCTION__ << " : checking cloned trajectories" << endl;
+	for (unsigned int i=0; i<v.size(); i++){
+		TNode* node= v[i];
+		while (node){
+			//cerr <<".";
+			node=node->parent;
+		}
+		//cerr << endl;
+	}	
+	
+	return v;
+
+}
+
+void GridSlamProcessor::integrateScanSequence(GridSlamProcessor::TNode* node){
+	//reverse the list 
+	TNode* aux=node;
+	TNode* reversed=0;
+	double count=0;
+	while(aux!=0){
+		TNode * newnode=new TNode(*aux);
+		newnode->parent=reversed;
+		reversed=newnode;
+		aux=aux->parent;
+		count++;
+	}
+	
+	//attach the path to each particle and compute the map;
+	if (m_infoStream )
+		m_infoStream << "Restoring State Nodes=" <<count << endl;
+		
+		
+	aux=reversed;
+	bool first=true;
+	double oldWeight=0;
+	OrientedPoint oldPose;
+	while (aux!=0){
+		if (first){
+			oldPose=aux->pose;
+			first=false;
+			oldWeight=aux->weight;
+		}
+		
+		OrientedPoint dp=aux->pose-oldPose;
+		double dw=aux->weight-oldWeight;
+		oldPose=aux->pose;
+		
+		
+		double * plainReading = new double[m_beams];
+		for(unsigned int i=0; i<m_beams; i++)
+			plainReading[i]=(*(aux->reading))[i];
+		
+		for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+			//compute the position relative to the path;
+			double s=sin(oldPose.theta-it->pose.theta),
+			       c=cos(oldPose.theta-it->pose.theta);
+			
+			it->pose.x+=c*dp.x-s*dp.y;
+			it->pose.y+=s*dp.x+c*dp.y;
+			it->pose.theta+=dp.theta;
+			it->pose.theta=atan2(sin(it->pose.theta), cos(it->pose.theta));
+			
+			//register the scan
+			m_matcher.invalidateActiveArea();
+			m_matcher.computeActiveArea(it->map, it->pose, plainReading);
+			it->weight+=dw;
+			it->weightSum+=dw;
+
+			// this should not work, since it->weight is not the correct weight!
+			//			it->node=new TNode(it->pose, it->weight, it->node);
+			it->node=new TNode(it->pose, 0.0, it->node);
+			//update the weight
+		}
+		
+		delete [] plainReading;
+		aux=aux->parent;
+	}
+	
+	//destroy the path
+	aux=reversed;
+	while (reversed){
+		aux=reversed;
+		reversed=reversed->parent;
+		delete aux;
+	}
+}
+
+//END State Save/Restore
+
+//BEGIN
+
+void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){
+
+  if (!weightsAlreadyNormalized) {
+    normalize();
+  }
+  resetTree();
+  propagateWeights();
+}
+
+void GridSlamProcessor::resetTree(){
+  // don't calls this function directly, use updateTreeWeights(..) !
+
+	for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+		TNode* n=it->node;
+		while (n){
+			n->accWeight=0;
+			n->visitCounter=0;
+			n=n->parent;
+		}
+	}
+}
+
+double propagateWeight(GridSlamProcessor::TNode* n, double weight){
+	if (!n)
+		return weight;
+	double w=0;
+	n->visitCounter++;
+	n->accWeight+=weight;
+	if (n->visitCounter==n->childs){
+		w=propagateWeight(n->parent,n->accWeight);
+	}
+	assert(n->visitCounter<=n->childs);
+	return w;
+}
+
+double GridSlamProcessor::propagateWeights(){
+  // don't calls this function directly, use updateTreeWeights(..) !
+
+        // all nodes must be resetted to zero and weights normalized
+
+        // the accumulated weight of the root
+	double lastNodeWeight=0;
+	// sum of the weights in the leafs
+	double aw=0;                   
+
+	std::vector<double>::iterator w=m_weights.begin();
+	for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+		double weight=*w;
+		aw+=weight;
+		TNode * n=it->node;
+		n->accWeight=weight;
+		lastNodeWeight+=propagateWeight(n->parent,n->accWeight);
+		w++;
+	}
+	
+	if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001) {
+	  cerr << "ERROR: ";
+	  cerr << "root->accWeight=" << lastNodeWeight << "    sum_leaf_weights=" << aw << endl;
+	  assert(0);         
+	}
+	return lastNodeWeight;
+}
+
+};
+
+//END

slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp

@@ -0,0 +1,80 @@
+#include <gmapping/gridfastslam/motionmodel.h>
+#include <gmapping/utils/stat.h>
+#include <iostream>
+
+#define MotionModelConditioningLinearCovariance 0.01
+#define MotionModelConditioningAngularCovariance 0.001
+
+namespace GMapping {
+
+
+
+OrientedPoint 
+MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{
+	OrientedPoint n(p);
+	double lm=linearMove  + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str );
+	double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt );
+	n.x+=lm*cos(n.theta+.5*am);
+	n.y+=lm*sin(n.theta+.5*am);
+	n.theta+=am;
+	n.theta=atan2(sin(n.theta), cos(n.theta));
+	return n;
+}
+
+OrientedPoint 
+MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
+	double sxy=0.3*srr;
+	OrientedPoint delta=absoluteDifference(pnew, pold);
+	OrientedPoint noisypoint(delta);
+	noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
+	noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
+	noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
+	noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
+	if (noisypoint.theta>M_PI)
+		noisypoint.theta-=2*M_PI;
+	return absoluteSum(p,noisypoint);
+}
+
+
+/*
+OrientedPoint 
+MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
+	
+	//compute the three stps needed for perfectly matching the two poses if the noise is absent
+	
+	OrientedPoint delta=pnew-pold;
+	double aoffset=atan2(delta.y, delta.x);
+	double alpha1=aoffset-pold.theta;
+	alpha1=atan2(sin(alpha1), cos(alpha1));
+	double rho=sqrt(delta*delta);
+	double alpha2=pnew.theta-aoffset;
+	alpha2=atan2(sin(alpha2), cos(alpha2));
+	
+	OrientedPoint pret=drawFromMotion(p, 0, alpha1);
+	pret=drawFromMotion(pret, rho, 0);
+	pret=drawFromMotion(pret, 0, alpha2);
+	return pret;
+}
+*/
+
+
+Covariance3 MotionModel::gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const{
+	OrientedPoint delta=absoluteDifference(pnew,pold);
+	double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y);
+	double angularMove=fabs(delta.x);
+	double s11=srr*srr*linearMove*linearMove;
+	double s22=stt*stt*angularMove*angularMove;
+	double s12=str*angularMove*srt*linearMove;
+	Covariance3 cov;
+	double s=sin(pold.theta),c=cos(pold.theta);
+	cov.xx=c*c*s11+MotionModelConditioningLinearCovariance;
+	cov.yy=s*s*s11+MotionModelConditioningLinearCovariance;
+	cov.tt=s22+MotionModelConditioningAngularCovariance;
+	cov.xy=s*c*s11;
+	cov.xt=c*s12;
+	cov.yt=s*s12;
+	return cov;
+}
+
+};
+