add slam_gmapping

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();
+}