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,97 @@
+#ifndef AUTOPTR_H
+#define AUTOPTR_H
+#include <assert.h>
+
+namespace GMapping{
+
+template <class X>
+class autoptr{
+	protected:
+	
+	public:
+	struct reference{
+		X* data;
+		unsigned int shares;
+	};
+		inline autoptr(X* p=(X*)(0));
+		inline autoptr(const autoptr<X>& ap);
+		inline autoptr& operator=(const autoptr<X>& ap);
+		inline ~autoptr();
+		inline operator int() const;
+		inline X& operator*();
+		inline const X& operator*() const;
+		//p	
+		reference * m_reference;
+	protected:
+};
+
+template <class X>
+autoptr<X>::autoptr(X* p){
+	m_reference=0;
+	if (p){
+		m_reference=new reference;
+		m_reference->data=p;
+		m_reference->shares=1;
+	}
+}
+
+template <class X>
+autoptr<X>::autoptr(const autoptr<X>& ap){
+	m_reference=0;
+	reference* ref=ap.m_reference;
+	if (ap.m_reference){
+		m_reference=ref;
+		m_reference->shares++;
+	}
+}
+
+template <class X>
+autoptr<X>& autoptr<X>::operator=(const autoptr<X>& ap){
+	reference* ref=ap.m_reference;
+	if (m_reference==ref){
+		return *this;
+	}
+	if (m_reference && !(--m_reference->shares)){
+		delete m_reference->data;
+		delete m_reference;
+		m_reference=0;
+	}	
+	if (ref){
+		m_reference=ref;
+		m_reference->shares++;
+	} 
+//20050802 nasty changes begin
+	else
+		m_reference=0;
+//20050802 nasty changes end
+	return *this;
+}
+
+template <class X>
+autoptr<X>::~autoptr(){
+	if (m_reference && !(--m_reference->shares)){
+		delete m_reference->data;
+		delete m_reference;
+		m_reference=0;
+	}	
+}
+
+template <class X>
+autoptr<X>::operator int() const{
+	return m_reference && m_reference->shares && m_reference->data;
+}
+
+template <class X>
+X& autoptr<X>::operator*(){
+	assert(m_reference && m_reference->shares && m_reference->data);
+	return *(m_reference->data);
+}
+
+template <class X>
+const X& autoptr<X>::operator*() const{
+	assert(m_reference && m_reference->shares && m_reference->data);
+	return *(m_reference->data);
+}
+
+};
+#endif
@@ -0,0 +1,115 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#ifndef COMMANDLINE_H
+#define COMMANDLINE_H
+
+	
+#define parseFlag(name,value)\
+if (!strcmp(argv[c],name)){\
+	value=true;\
+	cout << name << " on"<< endl;\
+	recognized=true;\
+}\
+
+#define parseString(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=argv[c];\
+	cout << name << "=" << value << endl;\
+	recognized=true;\
+}\
+
+
+#define parseDouble(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=atof(argv[c]);\
+	cout << name << "=" << value << endl;\
+	recognized=true;\
+}\
+
+#define parseInt(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=atoi(argv[c]);\
+	cout << name << "=" << value << endl;\
+	recognized=true;\
+}\
+
+#define CMD_PARSE_BEGIN(i, count)\
+{\
+	int c=i;\
+	while (c<count){\
+		bool recognized=false;
+	
+#define CMD_PARSE_END\
+		if (!recognized)\
+			cout << "COMMAND LINE: parameter " << argv[c] << " not recognized" << endl;\
+		c++;\
+	}\
+}
+
+#define CMD_PARSE_BEGIN_SILENT(i, count)\
+{\
+	int c=i;\
+	while (c<count){\
+		bool recognized=false;
+	
+#define CMD_PARSE_END_SILENT\
+		c++;\
+	}\
+}
+
+	
+#define parseFlagSilent(name,value)\
+if (!strcmp(argv[c],name)){\
+	value=true;\
+	recognized=true;\
+}\
+
+#define parseStringSilent(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=argv[c];\
+	recognized=true;\
+}\
+
+
+#define parseDoubleSilent(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=atof(argv[c]);\
+	recognized=true;\
+}\
+
+#define parseIntSilent(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=atoi(argv[c]);\
+	recognized=true;\
+}\
+
+
+#endif
+
@@ -0,0 +1,28 @@
+#ifndef _GVALUES_H_
+#define _GVALUES_H_
+
+#define MAXDOUBLE 1e1000
+#ifdef LINUX
+	#include <values.h>
+#endif
+#ifdef MACOSX
+	#include <limits.h>
+	#include <math.h>
+	//#define isnan(x) (x==FP_NAN)
+#endif
+#ifdef _WIN32
+  #include <limits>
+  #ifndef __DRAND48_DEFINED__
+     #define __DRAND48_DEFINED__
+     inline double drand48() { return double(rand()) / RAND_MAX;}
+  #endif
+  #ifndef M_PI
+    #define M_PI 3.1415926535897932384626433832795
+  #endif
+  #define round(d) (floor((d) + 0.5))
+  typedef unsigned int uint;
+  #define isnan(x) (_isnan(x))
+#endif
+
+#endif 
+
@@ -0,0 +1,38 @@
+#ifndef MACRO_PARAMS_H
+#define MACRO_PARAMS_H
+
+#define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)\
+qualifier: type m_##name;\
+getqualifier: inline type get##name() const {return m_##name;}\
+setqualifier: inline void set##name(type name) {m_##name=name;}
+
+#define PARAM_SET(type, name, qualifier, setqualifier)\
+qualifier: type m_##name;\
+setqualifier: inline void set##name(type name) {m_##name=name;}
+
+#define PARAM_GET(type, name, qualifier, getqualifier)\
+qualifier: type m_##name;\
+getqualifier: inline type get##name() const {return m_##name;}
+
+#define MEMBER_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\
+getqualifier: inline type get##name() const {return member.get##name();}\
+setqualifier: inline void set##name(type name) { member.set##name(name);}
+
+#define MEMBER_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\
+setqualifier: inline void set##name(type name) { member.set##name(name);}
+
+#define MEMBER_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\
+getqualifier: inline type get##name() const {return member.get##name();}
+
+#define STRUCT_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\
+getqualifier: inline type get##name() const {return member.name;}\
+setqualifier: inline void set##name(type name) {member.name=name;}
+
+#define STRUCT_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\
+setqualifier: inline void set##name(type name) {member.name=name;}
+
+#define STRUCT_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\
+getqualifier: inline type get##name() const {return member.name;}\
+
+#define convertStringArgument(var,val,buf) if (!strcmp(buf,#val)) var=val
+#endif
@@ -0,0 +1,207 @@
+#ifndef _POINT_H_
+#define _POINT_H_
+#include <assert.h>
+#include <math.h>
+#include <iostream>
+#include "gvalues.h"
+
+#define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME
+
+namespace GMapping {
+
+template <class T>
+struct point{
+	inline point():x(0),y(0) {}
+	inline point(T _x, T _y):x(_x),y(_y){}
+	T x, y;
+};
+
+template <class T>
+inline point<T> operator+(const point<T>& p1, const point<T>& p2){
+	return point<T>(p1.x+p2.x, p1.y+p2.y);
+}
+
+template <class T>
+inline point<T> operator - (const point<T> & p1, const point<T> & p2){
+	return point<T>(p1.x-p2.x, p1.y-p2.y);
+}
+
+template <class T>
+inline point<T> operator * (const point<T>& p, const T& v){
+	return point<T>(p.x*v, p.y*v);
+}
+
+template <class T>
+inline point<T> operator * (const T& v, const point<T>& p){
+	return point<T>(p.x*v, p.y*v);
+}
+
+template <class T>
+inline T operator * (const point<T>& p1, const point<T>& p2){
+	return p1.x*p2.x+p1.y*p2.y;
+}
+
+
+template <class T, class A>
+struct orientedpoint: public point<T>{
+  inline orientedpoint() : point<T>(0,0), theta(0) {};
+	inline orientedpoint(const point<T>& p);
+	inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
+        inline void normalize();
+	inline orientedpoint<T,A> rotate(A alpha){
+		T s=sin(alpha), c=cos(alpha);
+		A a=alpha+theta;
+		a=atan2(sin(a),cos(a));
+		return orientedpoint(
+			c*this->x-s*this->y,
+			s*this->x+c*this->y, 
+			a);
+	}
+	A theta;
+};
+
+
+template <class T, class A>
+void orientedpoint<T,A>::normalize() {
+  if (theta >= -M_PI && theta < M_PI)
+    return;
+  
+  int multiplier = (int)(theta / (2*M_PI));
+  theta = theta - multiplier*2*M_PI;
+  if (theta >= M_PI)
+    theta -= 2*M_PI;
+  if (theta < -M_PI)
+    theta += 2*M_PI;
+}
+
+
+template <class T, class A>
+orientedpoint<T,A>::orientedpoint(const point<T>& p){
+	this->x=p.x;
+	this->y=p.y;
+	this->theta=0.;
+}
+
+
+template <class T, class A>
+orientedpoint<T,A> operator+(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
+	return orientedpoint<T,A>(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta);
+}
+
+template <class T, class A>
+orientedpoint<T,A> operator - (const orientedpoint<T,A> & p1, const orientedpoint<T,A> & p2){
+	return orientedpoint<T,A>(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta);
+}
+
+template <class T, class A>
+orientedpoint<T,A> operator * (const orientedpoint<T,A>& p, const T& v){
+	return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
+}
+
+template <class T, class A>
+orientedpoint<T,A> operator * (const T& v, const orientedpoint<T,A>& p){
+	return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
+}
+
+template <class T, class A>
+orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
+	orientedpoint<T,A> delta=p1-p2;
+	delta.theta=atan2(sin(delta.theta), cos(delta.theta));
+	double s=sin(p2.theta), c=cos(p2.theta);
+	return orientedpoint<T,A>(c*delta.x+s*delta.y, 
+	                         -s*delta.x+c*delta.y, delta.theta);
+}
+
+template <class T, class A>
+orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
+	double s=sin(p1.theta), c=cos(p1.theta);
+	return orientedpoint<T,A>(c*p2.x-s*p2.y,
+				  s*p2.x+c*p2.y, p2.theta) + p1;
+}
+
+template <class T, class A>
+point<T> absoluteSum(const orientedpoint<T,A>& p1,const point<T>& p2){
+	double s=sin(p1.theta), c=cos(p1.theta);
+	return point<T>(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point<T>) p1;
+}
+
+template <class T>
+struct pointcomparator{
+	bool operator ()(const point<T>& a, const point<T>& b) const {
+		return a.x<b.x || (a.x==b.x && a.y<b.y);
+	}	
+};
+
+template <class T>
+struct pointradialcomparator{
+	point<T> origin;
+	bool operator ()(const point<T>& a, const point<T>& b) const {
+		point<T> delta1=a-origin;
+		point<T> delta2=b-origin;
+		return (atan2(delta1.y,delta1.x)<atan2(delta2.y,delta2.x));
+	}	
+};
+
+template <class T>
+inline point<T> max(const point<T>& p1, const point<T>& p2){
+	point<T> p=p1;
+	p.x=p.x>p2.x?p.x:p2.x;
+	p.y=p.y>p2.y?p.y:p2.y;
+	return p;
+}
+
+template <class T>
+inline point<T> min(const point<T>& p1, const point<T>& p2){
+	point<T> p=p1;
+	p.x=p.x<p2.x?p.x:p2.x;
+	p.y=p.y<p2.y?p.y:p2.y;
+	return p;
+}
+
+template <class T, class F>
+inline point<T> interpolate(const point<T>& p1,  const F& t1, const point<T>& p2, const F& t2, const F& t3){
+	F gain=(t3-t1)/(t2-t1);
+	point<T> p=p1+(p2-p1)*gain;
+	return p;
+}
+
+template <class T, class A, class F>
+inline orientedpoint<T,A> 
+interpolate(const orientedpoint<T,A>& p1,  const F& t1, const orientedpoint<T,A>& p2, const F& t2, const F& t3){
+	F gain=(t3-t1)/(t2-t1);
+	orientedpoint<T,A> p;
+	p.x=p1.x+(p2.x-p1.x)*gain;
+	p.y=p1.y+(p2.y-p1.y)*gain;
+	double  s=sin(p1.theta)+sin(p2.theta)*gain,
+		c=cos(p1.theta)+cos(p2.theta)*gain;
+	p.theta=atan2(s,c);
+	return p;
+}
+
+
+template <class T>
+inline double euclidianDist(const point<T>& p1, const point<T>& p2){
+  return hypot(p1.x-p2.x, p1.y-p2.y);
+}
+template <class T, class A>
+inline double euclidianDist(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
+  return hypot(p1.x-p2.x, p1.y-p2.y);
+}
+template <class T, class A>
+inline double euclidianDist(const orientedpoint<T,A>& p1, const point<T>& p2){
+  return hypot(p1.x-p2.x, p1.y-p2.y);
+}
+template <class T, class A>
+inline double euclidianDist(const point<T>& p1, const orientedpoint<T,A>& p2 ){
+  return hypot(p1.x-p2.x, p1.y-p2.y);
+}
+
+
+
+typedef point<int> IntPoint;
+typedef point<double> Point;
+typedef orientedpoint<double, double> OrientedPoint;
+
+}; //end namespace
+
+#endif
@@ -0,0 +1,147 @@
+#ifndef STAT_H
+#define STAT_H
+#include "point.h"
+#include <vector>
+#include "gvalues.h"
+
+namespace GMapping {
+
+/**stupid utility function for drawing particles form a zero mean, sigma variance normal distribution
+probably it should not go there*/
+double sampleGaussian(double sigma,unsigned int S=0);
+
+double evalGaussian(double sigmaSquare, double delta);
+double evalLogGaussian(double sigmaSquare, double delta);
+int sampleUniformInt(int max);
+double sampleUniformDouble(double min, double max);
+
+struct Covariance3{
+	Covariance3 operator + (const Covariance3 & cov) const;
+	static Covariance3 zero;
+	double xx, yy, tt, xy, xt, yt;
+};
+
+struct EigenCovariance3{
+	EigenCovariance3();
+	EigenCovariance3(const Covariance3& c);
+	EigenCovariance3 rotate(double angle) const;
+	OrientedPoint sample() const;
+	double eval[3];
+	double evec[3][3];
+};
+
+struct Gaussian3{
+	OrientedPoint mean;
+	EigenCovariance3 covariance;
+	Covariance3 cov;
+	double eval(const OrientedPoint& p) const;
+	void computeFromSamples(const std::vector<OrientedPoint> & poses);
+	void computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights );
+};
+
+template<typename PointIterator, typename WeightIterator>
+Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){
+	Gaussian3 gaussian;
+	OrientedPoint mean=OrientedPoint(0,0,0);
+	double wcum=0;
+	double s=0, c=0;
+	WeightIterator wt=weightBegin;
+	double *w=new double();
+	OrientedPoint *p=new OrientedPoint();
+	for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
+		*w=*wt;
+		*p=*pt;
+		s+=*w*sin(p->theta);
+		c+=*w*cos(p->theta);
+		mean.x+=*w*p->x;
+		mean.y+=*w*p->y;
+		wcum+=*w;
+		wt++;
+	}
+	mean.x/=wcum;
+	mean.y/=wcum;
+	s/=wcum;
+	c/=wcum;
+	mean.theta=atan2(s,c);
+
+	Covariance3 cov=Covariance3::zero;
+	wt=weightBegin;
+	for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
+		*w=*wt;
+		*p=*pt;
+		OrientedPoint delta=(*p)-mean;
+		delta.theta=atan2(sin(delta.theta),cos(delta.theta));
+		cov.xx+=*w*delta.x*delta.x;
+		cov.yy+=*w*delta.y*delta.y;
+		cov.tt+=*w*delta.theta*delta.theta;
+		cov.xy+=*w*delta.x*delta.y;
+		cov.yt+=*w*delta.y*delta.theta;
+		cov.xt+=*w*delta.x*delta.theta;
+		wt++;
+	}
+	cov.xx/=wcum;
+	cov.yy/=wcum;
+	cov.tt/=wcum;
+	cov.xy/=wcum;
+	cov.yt/=wcum;
+	cov.xt/=wcum;
+	EigenCovariance3 ecov(cov);
+	gaussian.mean=mean;
+	gaussian.covariance=ecov;
+	gaussian.cov=cov;
+	delete w;
+	delete p;
+	return gaussian;
+}
+
+template<typename PointIterator>
+Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){
+	Gaussian3 gaussian;
+	OrientedPoint mean=OrientedPoint(0,0,0);
+	double wcum=1;
+	double s=0, c=0;
+	OrientedPoint *p=new OrientedPoint();
+	for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
+		*p=*pt;
+		s+=sin(p->theta);
+		c+=cos(p->theta);
+		mean.x+=p->x;
+		mean.y+=p->y;
+		wcum+=1.;
+	}
+	mean.x/=wcum;
+	mean.y/=wcum;
+	s/=wcum;
+	c/=wcum;
+	mean.theta=atan2(s,c);
+
+	Covariance3 cov=Covariance3::zero;
+	for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
+		*p=*pt;
+		OrientedPoint delta=(*p)-mean;
+		delta.theta=atan2(sin(delta.theta),cos(delta.theta));
+		cov.xx+=delta.x*delta.x;
+		cov.yy+=delta.y*delta.y;
+		cov.tt+=delta.theta*delta.theta;
+		cov.xy+=delta.x*delta.y;
+		cov.yt+=delta.y*delta.theta;
+		cov.xt+=delta.x*delta.theta;
+	}
+	cov.xx/=wcum;
+	cov.yy/=wcum;
+	cov.tt/=wcum;
+	cov.xy/=wcum;
+	cov.yt/=wcum;
+	cov.xt/=wcum;
+	EigenCovariance3 ecov(cov);
+	gaussian.mean=mean;
+	gaussian.covariance=ecov;
+	gaussian.cov=cov;
+	delete p;
+	return gaussian;
+}
+
+
+}; //end namespace
+#endif
+