add slam_gmapping

2025-06-06 16:15:07 +08:00
parent 9187b9fb85
commit a7b75c31cb
141 changed files with 15992 additions and 0 deletions
@@ -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