add slam_gmapping

slam_gmapping/openslam_gmapping/grid/Makefile

@@ -0,0 +1,9 @@
+OBJS= 
+APPS= map_test
+
+LDFLAGS+= 
+CPPFLAGS+= -DNDEBUG 
+
+-include ../global.mk
+-include ../build_tools/Makefile.app
+

slam_gmapping/openslam_gmapping/grid/graphmap.cpp

@@ -0,0 +1,59 @@
+#ifndef GRAPHMAP_H
+#define GRAPHMAP_H
+#include <list>
+#include <utils/point.h>
+#include <utils/graph.h>
+#include <grid/map.h>
+
+namespace GMapping {
+
+class RasterMap;
+
+struct GraphMapPatch{
+	typedef typename std::list<IntPoint> PointList; 
+	/**Renders the map relatively to the center of the patch*/
+	//void render(RenderMap rmap);
+	/**returns the lower left corner of the patch, relative to the center*/
+	//Point minBoundary() const; 
+	/**returns the upper right corner of the patch, relative to the center*/
+	//Point maxBoundary() const; // 
+
+	OrientedPoint center;
+	PointList m_points;
+};
+
+struct Covariance3{
+	double sxx, sxy, sxt, syy, syt ,stt;
+};
+
+struct GraphMapEdge{
+	Covariance3 covariance;
+	GraphMapPatch* first, *second;
+	inline operator double() const{
+		return sqrt((first->center-second->center)*(first->center-second->center));
+	}
+};
+
+
+struct GraphPatchGraph: public Graph<GraphMapPatch, Covariance3>{
+	void addEdge(Vertex* v1, Vertex* v2, const Covariance3& covariance);
+};
+
+void GraphPatchGraph::addEdge(GraphPatchGraph::Vertex* v1, GraphPatchGraph::VertexVertex* v2, 
+	const Covariance3& cov){
+	GraphMapEdge gme;
+	gme.covariance=cov;
+	gme.first=v1;
+	gme.second=v2;
+	return Graph<GraphMapPatch, Covariance3>::addEdge(v1,v2,gme);
+}
+
+struct GraphPatchDirectoryCell: public std::set<GraphMapPatch::Vertex*> {
+	GraphPatchDirectoryCell(double);
+};
+
+typedef Map<GraphPatchDirectoryCell>, Array2D::set<GraphPatchDirectoryCell> >
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/grid/map_test.cpp

@@ -0,0 +1,62 @@
+#include <iostream>
+#include "map.h"
+#include "harray2d.h"
+
+using namespace std;
+using namespace GMapping;
+
+struct SimpleCell{
+	int value;
+	SimpleCell(int v=0){value=v;}
+	static const SimpleCell& Unknown();
+	static SimpleCell* address;
+};
+
+SimpleCell* SimpleCell::address=0;
+
+const SimpleCell& SimpleCell::Unknown(){
+	if (address)
+		return *address;
+	address=new SimpleCell(-1);
+	return *address;
+}
+
+typedef Map< SimpleCell, HierarchicalArray2D<SimpleCell> > CGrid;
+
+int main (int argc, char ** argv){
+	CGrid g1(Point(0.,0.), 200, 200, 0.1);
+	CGrid g2(Point(10.,10.), 200, 200, 0.1);
+	{
+		HierarchicalArray2D<SimpleCell>::PointSet ps;
+		IntPoint pp=g1.world2map(Point(5.1,5.1));
+		cout << pp.x << " " << pp.y << endl;
+		ps.insert(pp);
+		g1.storage().setActiveArea(ps,false);
+		g1.storage().allocActiveArea();
+		g1.cell(Point(5.1,5.1)).value=5;
+		cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl;
+		g1.resize(-150, -150, 150, 150);
+		cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl;
+		CGrid g3(g1);
+		g1=g2;
+	}
+	cerr << "copy and modify test" << endl;
+	CGrid *ap,* gp1=new CGrid(Point(0,0), 200, 200, 0.1);
+        CGrid* gp0=new CGrid(*gp1);
+	for (int i=1; i<10; i++){
+	  ap=new CGrid(*gp1);
+	  delete gp1;
+	  gp1=gp0;
+	  gp0=ap;
+	  IntPoint pp=gp0->world2map(Point(5.1,5.1));
+	  HierarchicalArray2D<SimpleCell>::PointSet ps;
+	  ps.insert(pp);
+          gp1->storage().setActiveArea(ps,false);	
+          gp1->storage().allocActiveArea();
+	  gp1->cell(Point(5.1,5.1)).value=i;
+          cout << "cell value" << (int) gp1->cell(Point(5.1,5.1)).value << endl;
+	}
+	delete gp0;
+	delete gp1;
+	return 0;
+}