m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add slam_gmapping

Browse Source
This commit is contained in:
X-lanni
2025-06-06 16:15:07 +08:00
parent 9187b9fb85
commit a7b75c31cb
141 changed files with 15992 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt
+4
View File
@@ -0,0 +1,4 @@
add_library(scanmatcher STATIC eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp)
target_link_libraries(scanmatcher sensor_range utils)
install(TARGETS scanmatcher DESTINATION lib)
slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp
+270
View File
@@ -0,0 +1,270 @@
/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
domain Java Matrix library JAMA. */
#include <math.h>
#ifndef MAX
#define MAX(a, b) ((a)>(b)?(a):(b))
#endif
#define n 3
static double hypot2(double x, double y) {
return sqrt(x*x+y*y);
}
// Symmetric Householder reduction to tridiagonal form.
static void tred2(double V[n][n], double d[n], double e[n]) {
// This is derived from the Algol procedures tred2 by
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
int i,j,k;
double f,g,h,hh;
for (j = 0; j < n; j++) {
d[j] = V[n-1][j];
}
// Householder reduction to tridiagonal form.
for (i = n-1; i > 0; i--) {
// Scale to avoid under/overflow.
double scale = 0.0;
double h = 0.0;
for (k = 0; k < i; k++) {
scale = scale + fabs(d[k]);
}
if (scale == 0.0) {
e[i] = d[i-1];
for (j = 0; j < i; j++) {
d[j] = V[i-1][j];
V[i][j] = 0.0;
V[j][i] = 0.0;
}
} else {
// Generate Householder vector.
for (k = 0; k < i; k++) {
d[k] /= scale;
h += d[k] * d[k];
}
f = d[i-1];
g = sqrt(h);
if (f > 0) {
g = -g;
}
e[i] = scale * g;
h = h - f * g;
d[i-1] = f - g;
for (j = 0; j < i; j++) {
e[j] = 0.0;
}
// Apply similarity transformation to remaining columns.
for (j = 0; j < i; j++) {
f = d[j];
V[j][i] = f;
g = e[j] + V[j][j] * f;
for (k = j+1; k <= i-1; k++) {
g += V[k][j] * d[k];
e[k] += V[k][j] * f;
}
e[j] = g;
}
f = 0.0;
for (j = 0; j < i; j++) {
e[j] /= h;
f += e[j] * d[j];
}
hh = f / (h + h);
for (j = 0; j < i; j++) {
e[j] -= hh * d[j];
}
for (j = 0; j < i; j++) {
f = d[j];
g = e[j];
for (k = j; k <= i-1; k++) {
V[k][j] -= (f * e[k] + g * d[k]);
}
d[j] = V[i-1][j];
V[i][j] = 0.0;
}
}
d[i] = h;
}
// Accumulate transformations.
for (i = 0; i < n-1; i++) {
V[n-1][i] = V[i][i];
V[i][i] = 1.0;
h = d[i+1];
if (h != 0.0) {
for (k = 0; k <= i; k++) {
d[k] = V[k][i+1] / h;
}
for (j = 0; j <= i; j++) {
g = 0.0;
for (k = 0; k <= i; k++) {
g += V[k][i+1] * V[k][j];
}
for (k = 0; k <= i; k++) {
V[k][j] -= g * d[k];
}
}
}
for (k = 0; k <= i; k++) {
V[k][i+1] = 0.0;
}
}
for (j = 0; j < n; j++) {
d[j] = V[n-1][j];
V[n-1][j] = 0.0;
}
V[n-1][n-1] = 1.0;
e[0] = 0.0;
}
// Symmetric tridiagonal QL algorithm.
static void tql2(double V[n][n], double d[n], double e[n]) {
// This is derived from the Algol procedures tql2, by
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
int i,j,m,l,k;
double g,p,r,dl1,h,f,tst1,eps;
double c,c2,c3,el1,s,s2;
for (i = 1; i < n; i++) {
e[i-1] = e[i];
}
e[n-1] = 0.0;
f = 0.0;
tst1 = 0.0;
eps = pow(2.0,-52.0);
for (l = 0; l < n; l++) {
// Find small subdiagonal element
tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
m = l;
while (m < n) {
if (fabs(e[m]) <= eps*tst1) {
break;
}
m++;
}
// If m == l, d[l] is an eigenvalue,
// otherwise, iterate.
if (m > l) {
int iter = 0;
do {
iter = iter + 1; // (Could check iteration count here.)
// Compute implicit shift
g = d[l];
p = (d[l+1] - g) / (2.0 * e[l]);
r = hypot2(p,1.0);
if (p < 0) {
r = -r;
}
d[l] = e[l] / (p + r);
d[l+1] = e[l] * (p + r);
dl1 = d[l+1];
h = g - d[l];
for (i = l+2; i < n; i++) {
d[i] -= h;
}
f = f + h;
// Implicit QL transformation.
p = d[m];
c = 1.0;
c2 = c;
c3 = c;
el1 = e[l+1];
s = 0.0;
s2 = 0.0;
for (i = m-1; i >= l; i--) {
c3 = c2;
c2 = c;
s2 = s;
g = c * e[i];
h = c * p;
r = hypot2(p,e[i]);
e[i+1] = s * r;
s = e[i] / r;
c = p / r;
p = c * d[i] - s * g;
d[i+1] = h + s * (c * g + s * d[i]);
// Accumulate transformation.
for (k = 0; k < n; k++) {
h = V[k][i+1];
V[k][i+1] = s * V[k][i] + c * h;
V[k][i] = c * V[k][i] - s * h;
}
}
p = -s * s2 * c3 * el1 * e[l] / dl1;
e[l] = s * p;
d[l] = c * p;
// Check for convergence.
} while (fabs(e[l]) > eps*tst1);
}
d[l] = d[l] + f;
e[l] = 0.0;
}
// Sort eigenvalues and corresponding vectors.
for (i = 0; i < n-1; i++) {
k = i;
p = d[i];
for (j = i+1; j < n; j++) {
if (d[j] < p) {
k = j;
p = d[j];
}
}
if (k != i) {
d[k] = d[i];
d[i] = p;
for (j = 0; j < n; j++) {
p = V[j][i];
V[j][i] = V[j][k];
V[j][k] = p;
}
}
}
}
void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
int i,j;
double e[n];
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
V[i][j] = A[i][j];
}
}
tred2(V, d, e);
tql2(V, d, e);
}
slam_gmapping/openslam_gmapping/scanmatcher/eig3.h
+11
View File
@@ -0,0 +1,11 @@
/* Eigen-decomposition for symmetric 3x3 real matrices.
Public domain, copied from the public domain Java library JAMA. */
#ifndef _eig_h
/* Symmetric matrix A => eigenvectors in columns of V, corresponding
eigenvalues in d. */
void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
#endif
slam_gmapping/openslam_gmapping/scanmatcher/gridlinetraversal.h
+128
View File
@@ -0,0 +1,128 @@
#ifndef GRIDLINETRAVERSAL_H
#define GRIDLINETRAVERSAL_H
#include <cstdlib>
#include <gmapping/utils/point.h>
namespace GMapping {
typedef struct {
int num_points;
IntPoint* points;
} GridLineTraversalLine;
struct GridLineTraversal {
inline static void gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ;
inline static void gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ;
};
void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line )
{
int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag;
int cnt = 0;
dx = abs(end.x-start.x); dy = abs(end.y-start.y);
if (dy <= dx) {
d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx);
if (start.x > end.x) {
x = end.x; y = end.y;
ydirflag = (-1);
xend = start.x;
} else {
x = start.x; y = start.y;
ydirflag = 1;
xend = end.x;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
if (((end.y - start.y) * ydirflag) > 0) {
while (x < xend) {
x++;
if (d <0) {
d+=incr1;
} else {
y++; d+=incr2;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
}
} else {
while (x < xend) {
x++;
if (d <0) {
d+=incr1;
} else {
y--; d+=incr2;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
}
}
} else {
d = 2*dx - dy;
incr1 = 2*dx; incr2 = 2 * (dx - dy);
if (start.y > end.y) {
y = end.y; x = end.x;
yend = start.y;
xdirflag = (-1);
} else {
y = start.y; x = start.x;
yend = end.y;
xdirflag = 1;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
if (((end.x - start.x) * xdirflag) > 0) {
while (y < yend) {
y++;
if (d <0) {
d+=incr1;
} else {
x++; d+=incr2;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
}
} else {
while (y < yend) {
y++;
if (d <0) {
d+=incr1;
} else {
x--; d+=incr2;
}
line->points[cnt].x=x;
line->points[cnt].y=y;
cnt++;
}
}
}
line->num_points = cnt;
}
void GridLineTraversal::gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) {
int i,j;
int half;
IntPoint v;
gridLineCore( start, end, line );
if ( start.x!=line->points[0].x ||
start.y!=line->points[0].y ) {
half = line->num_points/2;
for (i=0,j=line->num_points - 1;i<half; i++,j--) {
v = line->points[i];
line->points[i] = line->points[j];
line->points[j] = v;
}
}
}
};
#endif
slam_gmapping/openslam_gmapping/scanmatcher/icptest.cpp
+91
View File
@@ -0,0 +1,91 @@
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <list>
#include <gmapping/scanmatcher/icp.h>
using namespace GMapping;
using namespace std;
typedef std::list<PointPair> PointPairList;
PointPairList generateRandomPointPairs(int size, OrientedPoint t, double noise=0.){
PointPairList ppl;
double s=sin(t.theta), c=cos(t.theta);
for (int i=0; i<size; i++){
Point noiseDraw(noise*(drand48()-.5),noise*(drand48()-.5));
PointPair pp;
pp.first.x=100.*(drand48()-.5)+200;
pp.first.y=10.*(drand48()-.5);
pp.second.x= c*pp.first.x-s*pp.first.y;
pp.second.y= s*pp.first.x+c*pp.first.y;
pp.second=pp.second+t+noiseDraw;
//cerr << "p1=" << pp.first.x << " " << pp.first.y << endl;
//cerr << "p2=" << pp.second.x << " " << pp.second.y << endl;
ppl.push_back(pp);
}
return ppl;
}
int main(int argc, const char ** argv){
while (1){
OrientedPoint t;
int size;
cerr << "Insert size, t.x, t.y, t.theta" << endl;
cin >> size >> t.x >> t.y >> t.theta;
PointPairList ppl=generateRandomPointPairs(size, t, 3);
OrientedPoint tc;
OrientedPoint ttot(0.,0.,0.);
bool method=true;
while(1){
char buf[10];
cerr << "iterate?" << endl;
cin.getline(buf,10);
if (buf[0]=='n')
method=false;
else if (buf[0]=='l')
method=true;
else if (buf[0]!=char(0))
break;
cout << "plot '-' w l, '-' w p, '-' w p" << endl;
for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
cout << it->first.x << " " << it->first.y<< endl;
cout << it->second.x << " " << it->second.y<< endl;
cout << endl;
}
cout << "e" << endl;
for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
cout << it->first.x << " " << it->first.y<< endl;
}
cout << "e" << endl;
for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
cout << it->second.x << " " << it->second.y<< endl;
}
cout << "e" << endl;
double error;
if (!method){
cerr << "Nonlinear Optimization" << endl;
error=icpNonlinearStep(tc,ppl);
}else {
cerr << "Linear Optimization" << endl;
error=icpStep(tc,ppl);
}
cerr << "ICP err=" << error << " t.x=" << tc.x << " t.y=" << tc.y << " t.theta=" << tc.theta << endl;
cerr << "\t" << error << " ttot.x=" << ttot.x << " ttot.y=" << ttot.y << " ttot.theta=" << ttot.theta << endl;
double s=sin(tc.theta), c=cos(tc.theta);
for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
Point p1(c*it->first.x-s*it->first.y+tc.x,
s*it->first.x+c*it->first.y+tc.y);
it->first=p1;
}
ttot.x+=tc.x;
ttot.y+=tc.y;
ttot.theta+=tc.theta;
ttot.theta=atan2(sin(ttot.theta), cos(ttot.theta));
}
}
return 0;
}
slam_gmapping/openslam_gmapping/scanmatcher/lumiles.h
+50
View File
@@ -0,0 +1,50 @@
#ifndef LUMILESPROCESSOR
#define LUMILESPROCESSOR
namespace GMapping{
class LuMilesProcessor{
typedef std:vector<Point> PointVector;
static OrientedPoint step(const PointVector& src, const PointVector& dest);
};
OrientedPoint LuMilesProcessors::step(const PointVector& src, const PointVector& dest){
assert(src.size()==dest.size());
unsigned int size=dest.size();
double smx=0, smy=0, dmx=0, dmy=0;
for (PointVector::const_iterator it=src.begin(); it!=src.end(); it++){
smx+=it->x;
smy+=it->y;
}
smx/=src.size();
smy/=src.size();
for (PointVector::const_iterator it=dest.begin(); it!=dest.end(); it++){
dmx+=it->x;
dmy+=it->y;
}
dmx/=src.size();
dmy/=src.size();
double sxx=0, sxy=0;
double syx=0, syy=0;
for (unsigned int i=0; i<size(); i++){
sxx+=(src[i].x-smx)*(dest[i].x-dmx);
sxy+=(src[i].x-smx)*(dest[i].y-dmy);
syx+=(src[i].y-smy)*(dest[i].x-dmx);
syy+=(src[i].y-smy)*(dest[i].y-dmy);
}
double omega=atan2(sxy-syx,sxx+syy);
return OrientedPoint(
dmx-smx*cos(omega)+smx*sin(omega)),
dmy-smx*sin(omega)-smy*cos(omega)),
omega
)
};
int main(int argc, conat char ** argv){
}
};
#endif
slam_gmapping/openslam_gmapping/scanmatcher/scanmatch_test.cpp
+186
View File
@@ -0,0 +1,186 @@
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <log/carmenconfiguration.h>
#include <log/sensorlog.h>
#include <unistd.h>
#include <utils/commandline.h>
#include <log/sensorstream.h>
#include "scanmatcherprocessor.h"
using namespace std;
using namespace GMapping;
#define DEBUG cout << __PRETTY_FUNCTION__
#define MAX_STRING_LENGTH 1024
int main(int argc, const char * const * argv){
string filename;
string outfilename;
double xmin=-100.;
double ymin=-100.;
double xmax=100.;
double ymax=100.;
double delta=1.;
double patchDelta=0.1;
double sigma=0.02;
double maxrange=81.9;
double maxUrange=81.9;
double regscore=1e4;
double lstep=.05;
double astep=.05;
int kernelSize=0;
int iterations=4;
double critscore=0.;
double maxMove=1.;
bool computeCovariance=false;
bool readFromStdin=false;
bool useICP=false;
double laserx=.0,lasery=.0,lasertheta=.0;
// bool headingOnly=false;
if (argc<2){
cout << "usage main {arglist}" << endl;
cout << "where the arguments are: " << endl;
cout << "\t -xmin <value>" << endl;
cout << "\t -xmax <value>" << endl;
cout << "\t -ymin <value>" << endl;
cout << "\t -ymax <value>" << endl;
cout << "\t -maxrange <value> : maxmimum preception range" << endl;
cout << "\t -delta <value> : patch size" << endl;
cout << "\t -patchDelta <value> : patch cell size" << endl;
cout << "\t -lstep <value> : linear serach step" << endl;
cout << "\t -astep <value> : ìangular search step" << endl;
cout << "\t -regscore <value> : registration scan score" << endl;
cout << "\t -filename <value> : log filename in carmen format" << endl;
cout << "\t -sigma <value> : convolution kernel size" << endl;
cout << "Look the code for discovering another thousand of unuseful parameters" << endl;
return -1;
}
CMD_PARSE_BEGIN(1,argc);
parseString("-filename",filename);
parseString("-outfilename",outfilename);
parseDouble("-xmin",xmin);
parseDouble("-xmax",xmax);
parseDouble("-ymin",ymin);
parseDouble("-ymax",ymax);
parseDouble("-delta",delta);
parseDouble("-patchDelta",patchDelta);
parseDouble("-maxrange",maxrange);
parseDouble("-maxUrange",maxUrange);
parseDouble("-regscore",regscore);
parseDouble("-critscore",critscore);
parseInt("-kernelSize",kernelSize);
parseDouble("-sigma",sigma);
parseInt("-iterations",iterations);
parseDouble("-lstep",lstep);
parseDouble("-astep",astep);
parseDouble("-maxMove",maxMove);
parseFlag("-computeCovariance",computeCovariance);
parseFlag("-stdin", readFromStdin);
parseFlag("-useICP", useICP);
parseDouble("-laserx",laserx);
parseDouble("-lasery",lasery);
parseDouble("-lasertheta",lasertheta);
CMD_PARSE_END;
if (!filename.size()){
cout << "no filename specified" << endl;
return -1;
}
ifstream is;
is.open(filename.c_str());
if (! is){
cout << "no file found" << endl;
return -1;
}
DEBUG << "scanmatcher processor construction" << endl;
ScanMatcherProcessor scanmatcher(xmin, ymin, xmax, ymax, delta, patchDelta);
//double range, double sigma, int kernsize, double lopt, double aopt, int iterations
scanmatcher.setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, computeCovariance);
scanmatcher.setRegistrationParameters(regscore, critscore);
scanmatcher.setmaxMove(maxMove);
scanmatcher.useICP=useICP;
scanmatcher.matcher().setlaserPose(OrientedPoint(laserx,lasery,lasertheta));
CarmenConfiguration conf;
conf.load(is);
is.close();
SensorMap sensorMap=conf.computeSensorMap();
scanmatcher.setSensorMap(sensorMap);
InputSensorStream* input=0;
ifstream plainStream;
if (! readFromStdin){
plainStream.open(filename.c_str());
input=new InputSensorStream(sensorMap, plainStream);
cout << "Plain Stream opened="<< (bool) plainStream << endl;
} else {
input=new InputSensorStream(sensorMap, cin);
cout << "Plain Stream opened on stdin" << endl;
}
/*
SensorLog log(sensorMap);
ifstream logstream(filename);
log.load(logstream);
logstream.close();
cout << "Log loaded " << log.size() << " records" << endl;
*/
ostream* output;
ofstream poseStream;
if (! readFromStdin){
if (! outfilename.size()){
outfilename=string("scanmatched")+filename;
}
poseStream.open(outfilename.c_str());
output=&poseStream;
} else {
output=&cout;
}
scanmatcher.init();
ofstream odopathStream("odopath.dat");
while (*input){
const SensorReading* r;
(*input) >> r;
if (! r)
continue;
const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
if (rr){
const RangeSensor* s=dynamic_cast<const RangeSensor*>(r->getSensor());
bool isFront= s->getPose().theta==0;
if (! readFromStdin){
cout << "." << flush;
}
const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
assert (rs && rs->beams().size()==rr->size());
odopathStream << rr->getPose().x << " " << rr->getPose().y << endl;
scanmatcher.processScan(*rr);
OrientedPoint p=scanmatcher.getPose();
if (isFront)
*output << "FLASER "<< rr->size() << " ";
else
*output << "RLASER "<< rr->size() << " ";
for (RangeReading::const_iterator b=rr->begin(); b!=rr->end(); b++){
*output << *b << " ";
}
*output << p.x << " " << p.y << " " << p.theta << " ";
//p=rr->getPose();
double t=rr->getTime(); //FIXME
*output << p.x << " " << p.y << " " << p.theta << " ";
*output << t << " nohost " << t << endl;
}
}
if (! readFromStdin){
poseStream.close();
}
}
slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.cpp
+725
View File
@@ -0,0 +1,725 @@
#include <cstring>
#include <limits>
#include <list>
#include <iostream>
#include <gmapping/scanmatcher/scanmatcher.h>
#include "gridlinetraversal.h"
//#define GENERATE_MAPS
namespace GMapping {
using namespace std;
const double ScanMatcher::nullLikelihood=-.5;
ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){
//m_laserAngles=0;
m_laserBeams=0;
m_optRecursiveIterations=3;
m_activeAreaComputed=false;
// This are the dafault settings for a grid map of 5 cm
m_llsamplerange=0.01;
m_llsamplestep=0.01;
m_lasamplerange=0.005;
m_lasamplestep=0.005;
m_enlargeStep=10.;
m_fullnessThreshold=0.1;
m_angularOdometryReliability=0.;
m_linearOdometryReliability=0.;
m_freeCellRatio=sqrt(2.);
m_initialBeamsSkip=0;
/*
// This are the dafault settings for a grid map of 10 cm
m_llsamplerange=0.1;
m_llsamplestep=0.1;
m_lasamplerange=0.02;
m_lasamplestep=0.01;
*/
// This are the dafault settings for a grid map of 20/25 cm
/*
m_llsamplerange=0.2;
m_llsamplestep=0.1;
m_lasamplerange=0.02;
m_lasamplestep=0.01;
m_generateMap=false;
*/
m_linePoints = new IntPoint[20000];
}
ScanMatcher::~ScanMatcher(){
delete [] m_linePoints;
}
void ScanMatcher::invalidateActiveArea(){
m_activeAreaComputed=false;
}
/*
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
if (m_activeAreaComputed)
return;
HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
const double * angle=m_laserAngles;
for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
if (m_generateMap){
double d=*r;
if (d>m_laserMaxRange)
continue;
if (d>m_usableRange)
d=m_usableRange;
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p1=map.world2map(phit);
d+=map.getDelta();
//Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
//IntPoint p2=map.world2map(phit2);
IntPoint linePoints[20000] ;
GridLineTraversalLine line;
line.points=linePoints;
//GridLineTraversal::gridLine(p0, p2, &line);
GridLineTraversal::gridLine(p0, p1, &line);
for (int i=0; i<line.num_points-1; i++){
activeArea.insert(map.storage().patchIndexes(linePoints[i]));
}
if (d<=m_usableRange){
activeArea.insert(map.storage().patchIndexes(p1));
//activeArea.insert(map.storage().patchIndexes(p2));
}
} else {
if (*r>m_laserMaxRange||*r>m_usableRange) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
}
//this allocates the unallocated cells in the active area of the map
//cout << "activeArea::size() " << activeArea.size() << endl;
map.storage().setActiveArea(activeArea, true);
m_activeAreaComputed=true;
}
*/
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
if (m_activeAreaComputed)
return;
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
Point min(map.map2world(0,0));
Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
if (lp.x<min.x) min.x=lp.x;
if (lp.y<min.y) min.y=lp.y;
if (lp.x>max.x) max.x=lp.x;
if (lp.y>max.y) max.y=lp.y;
/*determine the size of the area*/
const double * angle=m_laserAngles+m_initialBeamsSkip;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
double d=*r>m_usableRange?m_usableRange:*r;
Point phit=lp;
phit.x+=d*cos(lp.theta+*angle);
phit.y+=d*sin(lp.theta+*angle);
if (phit.x<min.x) min.x=phit.x;
if (phit.y<min.y) min.y=phit.y;
if (phit.x>max.x) max.x=phit.x;
if (phit.y>max.y) max.y=phit.y;
}
//min=min-Point(map.getDelta(),map.getDelta());
//max=max+Point(map.getDelta(),map.getDelta());
if ( !map.isInside(min) || !map.isInside(max)){
Point lmin(map.map2world(0,0));
Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
//cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl;
//cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep;
max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep;
min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep;
max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep;
map.resize(min.x, min.y, max.x, max.y);
//cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
}
HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
/*allocate the active area*/
angle=m_laserAngles+m_initialBeamsSkip;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
if (m_generateMap){
double d=*r;
if (d>m_laserMaxRange||d==0.0||isnan(d))
continue;
if (d>m_usableRange)
d=m_usableRange;
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p0=map.world2map(lp);
IntPoint p1=map.world2map(phit);
//IntPoint linePoints[20000] ;
GridLineTraversalLine line;
line.points=m_linePoints;
GridLineTraversal::gridLine(p0, p1, &line);
for (int i=0; i<line.num_points-1; i++){
assert(map.isInside(m_linePoints[i]));
activeArea.insert(map.storage().patchIndexes(m_linePoints[i]));
assert(m_linePoints[i].x>=0 && m_linePoints[i].y>=0);
}
if (d<m_usableRange){
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
}
} else {
if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
}
//this allocates the unallocated cells in the active area of the map
//cout << "activeArea::size() " << activeArea.size() << endl;
/*
cerr << "ActiveArea=";
for (HierarchicalArray2D<PointAccumulator>::PointSet::const_iterator it=activeArea.begin(); it!= activeArea.end(); it++){
cerr << "(" << it->x <<"," << it->y << ") ";
}
cerr << endl;
*/
map.storage().setActiveArea(activeArea, true);
m_activeAreaComputed=true;
}
double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
if (!m_activeAreaComputed)
computeActiveArea(map, p, readings);
//this operation replicates the cells that will be changed in the registration operation
map.storage().allocActiveArea();
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
const double * angle=m_laserAngles+m_initialBeamsSkip;
double esum=0;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
if (m_generateMap){
double d=*r;
if (d>m_laserMaxRange||d==0.0||isnan(d))
continue;
if (d>m_usableRange)
d=m_usableRange;
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p1=map.world2map(phit);
//IntPoint linePoints[20000] ;
GridLineTraversalLine line;
line.points=m_linePoints;
GridLineTraversal::gridLine(p0, p1, &line);
for (int i=0; i<line.num_points-1; i++){
PointAccumulator& cell=map.cell(line.points[i]);
double e=-cell.entropy();
cell.update(false, Point(0,0));
e+=cell.entropy();
esum+=e;
}
if (d<m_usableRange){
double e=-map.cell(p1).entropy();
map.cell(p1).update(true, phit);
e+=map.cell(p1).entropy();
esum+=e;
}
} else {
if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
map.cell(p1).update(true,phit);
}
//cout << "informationGain=" << -esum << endl;
return esum;
}
/*
void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
if (!m_activeAreaComputed)
computeActiveArea(map, p, readings);
//this operation replicates the cells that will be changed in the registration operation
map.storage().allocActiveArea();
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
const double * angle=m_laserAngles;
for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
if (m_generateMap){
double d=*r;
if (d>m_laserMaxRange)
continue;
if (d>m_usableRange)
d=m_usableRange;
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p1=map.world2map(phit);
IntPoint linePoints[20000] ;
GridLineTraversalLine line;
line.points=linePoints;
GridLineTraversal::gridLine(p0, p1, &line);
for (int i=0; i<line.num_points-1; i++){
IntPoint ci=map.storage().patchIndexes(line.points[i]);
if (map.storage().getActiveArea().find(ci)==map.storage().getActiveArea().end())
cerr << "BIG ERROR" <<endl;
map.cell(line.points[i]).update(false, Point(0,0));
}
if (d<=m_usableRange){
map.cell(p1).update(true,phit);
}
} else {
if (*r>m_laserMaxRange||*r>m_usableRange) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
map.cell(phit).update(true,phit);
}
}
*/
double ScanMatcher::icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
double currentScore;
double sc=score(map, init, readings);;
OrientedPoint start=init;
pnew=init;
int iterations=0;
do{
currentScore=sc;
sc=icpStep(pnew, map, start, readings);
//cerr << "pstart=" << start.x << " " <<start.y << " " << start.theta << endl;
//cerr << "pret=" << pnew.x << " " <<pnew.y << " " << pnew.theta << endl;
start=pnew;
iterations++;
} while (sc>currentScore);
cerr << "i="<< iterations << endl;
return currentScore;
}
double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
double bestScore=-1;
OrientedPoint currentPose=init;
double currentScore=score(map, currentPose, readings);
double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
unsigned int refinement=0;
enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
/* cout << __PRETTY_FUNCTION__<< " readings: ";
for (int i=0; i<m_laserBeams; i++){
cout << readings[i] << " ";
}
cout << endl;
*/ int c_iterations=0;
do{
if (bestScore>=currentScore){
refinement++;
adelta*=.5;
ldelta*=.5;
}
bestScore=currentScore;
// cout <<"score="<< currentScore << " refinement=" << refinement;
// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
OrientedPoint bestLocalPose=currentPose;
OrientedPoint localPose=currentPose;
Move move=Front;
do {
localPose=currentPose;
switch(move){
case Front:
localPose.x+=ldelta;
move=Back;
break;
case Back:
localPose.x-=ldelta;
move=Left;
break;
case Left:
localPose.y-=ldelta;
move=Right;
break;
case Right:
localPose.y+=ldelta;
move=TurnLeft;
break;
case TurnLeft:
localPose.theta+=adelta;
move=TurnRight;
break;
case TurnRight:
localPose.theta-=adelta;
move=Done;
break;
default:;
}
double odo_gain=1;
if (m_angularOdometryReliability>0.){
double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth;
odo_gain*=exp(-m_angularOdometryReliability*dth);
}
if (m_linearOdometryReliability>0.){
double dx=init.x-localPose.x;
double dy=init.y-localPose.y;
double drho=dx*dx+dy*dy;
odo_gain*=exp(-m_linearOdometryReliability*drho);
}
double localScore=odo_gain*score(map, localPose, readings);
if (localScore>currentScore){
currentScore=localScore;
bestLocalPose=localPose;
}
c_iterations++;
} while(move!=Done);
currentPose=bestLocalPose;
// cout << "currentScore=" << currentScore<< endl;
//here we look for the best move;
}while (currentScore>bestScore || refinement<m_optRecursiveIterations);
//cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
//cout << __PRETTY_FUNCTION__ << "iterations=" << c_iterations<< endl;
pnew=currentPose;
return bestScore;
}
struct ScoredMove{
OrientedPoint pose;
double score;
double likelihood;
};
typedef std::list<ScoredMove> ScoredMoveList;
double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
ScoredMoveList moveList;
double bestScore=-1;
OrientedPoint currentPose=init;
ScoredMove sm={currentPose,0,0};
unsigned int matched=likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
double currentScore=sm.score;
moveList.push_back(sm);
double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
unsigned int refinement=0;
int count=0;
enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
do{
if (bestScore>=currentScore){
refinement++;
adelta*=.5;
ldelta*=.5;
}
bestScore=currentScore;
// cout <<"score="<< currentScore << " refinement=" << refinement;
// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
OrientedPoint bestLocalPose=currentPose;
OrientedPoint localPose=currentPose;
Move move=Front;
do {
localPose=currentPose;
switch(move){
case Front:
localPose.x+=ldelta;
move=Back;
break;
case Back:
localPose.x-=ldelta;
move=Left;
break;
case Left:
localPose.y-=ldelta;
move=Right;
break;
case Right:
localPose.y+=ldelta;
move=TurnLeft;
break;
case TurnLeft:
localPose.theta+=adelta;
move=TurnRight;
break;
case TurnRight:
localPose.theta-=adelta;
move=Done;
break;
default:;
}
double localScore, localLikelihood;
double odo_gain=1;
if (m_angularOdometryReliability>0.){
double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth;
odo_gain*=exp(-m_angularOdometryReliability*dth);
}
if (m_linearOdometryReliability>0.){
double dx=init.x-localPose.x;
double dy=init.y-localPose.y;
double drho=dx*dx+dy*dy;
odo_gain*=exp(-m_linearOdometryReliability*drho);
}
localScore=odo_gain*score(map, localPose, readings);
//update the score
count++;
matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);
if (localScore>currentScore){
currentScore=localScore;
bestLocalPose=localPose;
}
sm.score=localScore;
sm.likelihood=localLikelihood;//+log(odo_gain);
sm.pose=localPose;
moveList.push_back(sm);
//update the move list
} while(move!=Done);
currentPose=bestLocalPose;
//cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl;
//here we look for the best move;
}while (currentScore>bestScore || refinement<m_optRecursiveIterations);
//cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
//cout << __PRETTY_FUNCTION__ << "iterations=" << count<< endl;
//normalize the likelihood
double lmin=1e9;
double lmax=-1e9;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
lmin=it->likelihood<lmin?it->likelihood:lmin;
lmax=it->likelihood>lmax?it->likelihood:lmax;
}
//cout << "lmin=" << lmin << " lmax=" << lmax<< endl;
for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
it->likelihood=exp(it->likelihood-lmax);
//cout << "l=" << it->likelihood << endl;
}
//compute the mean
OrientedPoint mean(0,0,0);
double lacc=0;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
mean=mean+it->pose*it->likelihood;
lacc+=it->likelihood;
}
mean=mean*(1./lacc);
//OrientedPoint delta=mean-currentPose;
//cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
OrientedPoint delta=it->pose-mean;
delta.theta=atan2(sin(delta.theta), cos(delta.theta));
cov.xx+=delta.x*delta.x*it->likelihood;
cov.yy+=delta.y*delta.y*it->likelihood;
cov.tt+=delta.theta*delta.theta*it->likelihood;
cov.xy+=delta.x*delta.y*it->likelihood;
cov.xt+=delta.x*delta.theta*it->likelihood;
cov.yt+=delta.y*delta.theta*it->likelihood;
}
cov.xx/=lacc, cov.xy/=lacc, cov.xt/=lacc, cov.yy/=lacc, cov.yt/=lacc, cov.tt/=lacc;
_mean=currentPose;
_cov=cov;
return bestScore;
}
void ScanMatcher::setLaserParameters
(unsigned int beams, double* angles, const OrientedPoint& lpose){
/*if (m_laserAngles)
delete [] m_laserAngles;
*/
assert(beams<LASER_MAXBEAMS);
m_laserPose=lpose;
m_laserBeams=beams;
//m_laserAngles=new double[beams];
memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);
}
double ScanMatcher::likelihood
(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
ScoredMoveList moveList;
for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
OrientedPoint rp=p;
rp.x+=xx;
rp.y+=yy;
rp.theta+=tt;
ScoredMove sm;
sm.pose=rp;
likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
moveList.push_back(sm);
}
//OrientedPoint delta=mean-currentPose;
//cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
//normalize the likelihood
double lmax=-1e9;
double lcum=0;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
lmax=it->likelihood>lmax?it->likelihood:lmax;
}
for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
//it->likelihood=exp(it->likelihood-lmax);
lcum+=exp(it->likelihood-lmax);
it->likelihood=exp(it->likelihood-lmax);
//cout << "l=" << it->likelihood << endl;
}
OrientedPoint mean(0,0,0);
double s=0,c=0;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
mean=mean+it->pose*it->likelihood;
s+=it->likelihood*sin(it->pose.theta);
c+=it->likelihood*cos(it->pose.theta);
}
mean=mean*(1./lcum);
s/=lcum;
c/=lcum;
mean.theta=atan2(s,c);
CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
OrientedPoint delta=it->pose-mean;
delta.theta=atan2(sin(delta.theta), cos(delta.theta));
cov.xx+=delta.x*delta.x*it->likelihood;
cov.yy+=delta.y*delta.y*it->likelihood;
cov.tt+=delta.theta*delta.theta*it->likelihood;
cov.xy+=delta.x*delta.y*it->likelihood;
cov.xt+=delta.x*delta.theta*it->likelihood;
cov.yt+=delta.y*delta.theta*it->likelihood;
}
cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
_mean=mean;
_cov=cov;
_lmax=lmax;
return log(lcum)+lmax;
}
double ScanMatcher::likelihood
(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p,
Gaussian3& odometry, const double* readings, double gain){
ScoredMoveList moveList;
for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
OrientedPoint rp=p;
rp.x+=xx;
rp.y+=yy;
rp.theta+=tt;
ScoredMove sm;
sm.pose=rp;
likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
sm.likelihood+=odometry.eval(rp)/gain;
assert(!isnan(sm.likelihood));
moveList.push_back(sm);
}
//OrientedPoint delta=mean-currentPose;
//cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
//normalize the likelihood
double lmax=-std::numeric_limits<double>::max();
double lcum=0;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
lmax=it->likelihood>lmax?it->likelihood:lmax;
}
for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
//it->likelihood=exp(it->likelihood-lmax);
lcum+=exp(it->likelihood-lmax);
it->likelihood=exp(it->likelihood-lmax);
//cout << "l=" << it->likelihood << endl;
}
OrientedPoint mean(0,0,0);
double s=0,c=0;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
mean=mean+it->pose*it->likelihood;
s+=it->likelihood*sin(it->pose.theta);
c+=it->likelihood*cos(it->pose.theta);
}
mean=mean*(1./lcum);
s/=lcum;
c/=lcum;
mean.theta=atan2(s,c);
CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
OrientedPoint delta=it->pose-mean;
delta.theta=atan2(sin(delta.theta), cos(delta.theta));
cov.xx+=delta.x*delta.x*it->likelihood;
cov.yy+=delta.y*delta.y*it->likelihood;
cov.tt+=delta.theta*delta.theta*it->likelihood;
cov.xy+=delta.x*delta.y*it->likelihood;
cov.xt+=delta.x*delta.theta*it->likelihood;
cov.yt+=delta.y*delta.theta*it->likelihood;
}
cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
_mean=mean;
_cov=cov;
_lmax=lmax;
double v=log(lcum)+lmax;
assert(!isnan(v));
return v;
}
void ScanMatcher::setMatchingParameters
(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma, unsigned int likelihoodSkip){
m_usableRange=urange;
m_laserMaxRange=range;
m_kernelSize=kernsize;
m_optLinearDelta=lopt;
m_optAngularDelta=aopt;
m_optRecursiveIterations=iterations;
m_gaussianSigma=sigma;
m_likelihoodSigma=likelihoodSigma;
m_likelihoodSkip=likelihoodSkip;
}
};
slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.new.cpp
+426
View File
@@ -0,0 +1,426 @@
#include <list>
#include <iostream>
#include <gmapping/scanmatcher/scanmatcher.h>
#include "gridlinetraversal.h"
//#define GENERATE_MAPS
using namespace std;
namespace GMapping {
const double ScanMatcher::nullLikelihood=-1.;
ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){
m_laserAngles=0;
m_laserBeams=0;
m_optRecursiveIterations=3;
m_activeAreaComputed=false;
// This are the dafault settings for a grid map of 5 cm
m_llsamplerange=0.01;
m_llsamplestep=0.01;
m_lasamplerange=0.005;
m_lasamplestep=0.005;
/*
// This are the dafault settings for a grid map of 10 cm
m_llsamplerange=0.1;
m_llsamplestep=0.1;
m_lasamplerange=0.02;
m_lasamplestep=0.01;
*/
// This are the dafault settings for a grid map of 20/25 cm
/*
m_llsamplerange=0.2;
m_llsamplestep=0.1;
m_lasamplerange=0.02;
m_lasamplestep=0.01;
m_generateMap=false;
*/
}
ScanMatcher::~ScanMatcher(){
if (m_laserAngles)
delete [] m_laserAngles;
}
void ScanMatcher::invalidateActiveArea(){
m_activeAreaComputed=false;
}
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
if (m_activeAreaComputed)
return;
HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
const double * angle=m_laserAngles;
for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
if (m_generateMap){
double d=*r;
if (d>m_laserMaxRange)
continue;
if (d>m_usableRange)
d=m_usableRange;
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p1=map.world2map(phit);
d+=map.getDelta();
//Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
//IntPoint p2=map.world2map(phit2);
IntPoint linePoints[20000] ;
GridLineTraversalLine line;
line.points=linePoints;
//GridLineTraversal::gridLine(p0, p2, &line);
GridLineTraversal::gridLine(p0, p1, &line);
for (int i=0; i<line.num_points-1; i++){
activeArea.insert(map.storage().patchIndexes(linePoints[i]));
}
if (d<=m_usableRange){
activeArea.insert(map.storage().patchIndexes(p1));
//activeArea.insert(map.storage().patchIndexes(p2));
}
} else {
if (*r>m_laserMaxRange||*r>m_usableRange) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
}
//this allocates the unallocated cells in the active area of the map
//cout << "activeArea::size() " << activeArea.size() << endl;
map.storage().setActiveArea(activeArea, true);
m_activeAreaComputed=true;
}
void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
if (!m_activeAreaComputed)
computeActiveArea(map, p, readings);
//this operation replicates the cells that will be changed in the registration operation
map.storage().allocActiveArea();
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
const double * angle=m_laserAngles;
for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
if (m_generateMap){
double d=*r;
if (d>m_laserMaxRange)
continue;
if (d>m_usableRange)
d=m_usableRange;
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p1=map.world2map(phit);
d+=map.getDelta();
//Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
//IntPoint p2=map.world2map(phit2);
IntPoint linePoints[20000] ;
GridLineTraversalLine line;
line.points=linePoints;
//GridLineTraversal::gridLine(p0, p2, &line);
GridLineTraversal::gridLine(p0, p1, &line);
for (int i=0; i<line.num_points-1; i++){
map.cell(line.points[i]).update(false, Point(0,0));
}
if (d<=m_usableRange){
map.cell(p1).update(true,phit);
// map.cell(p2).update(true,phit);
}
} else {
if (*r>m_laserMaxRange||*r>m_usableRange) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
map.cell(phit).update(true,phit);
}
}
double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
double bestScore=-1;
OrientedPoint currentPose=init;
double currentScore=score(map, currentPose, readings);
double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
unsigned int refinement=0;
enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
int c_iterations=0;
do{
if (bestScore>=currentScore){
refinement++;
adelta*=.5;
ldelta*=.5;
}
bestScore=currentScore;
// cout <<"score="<< currentScore << " refinement=" << refinement;
// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
OrientedPoint bestLocalPose=currentPose;
OrientedPoint localPose=currentPose;
Move move=Front;
do {
localPose=currentPose;
switch(move){
case Front:
localPose.x+=ldelta;
move=Back;
break;
case Back:
localPose.x-=ldelta;
move=Left;
break;
case Left:
localPose.y-=ldelta;
move=Right;
break;
case Right:
localPose.y+=ldelta;
move=TurnLeft;
break;
case TurnLeft:
localPose.theta+=adelta;
move=TurnRight;
break;
case TurnRight:
localPose.theta-=adelta;
move=Done;
break;
default:;
}
double localScore=score(map, localPose, readings);
if (localScore>currentScore){
currentScore=localScore;
bestLocalPose=localPose;
}
c_iterations++;
} while(move!=Done);
currentPose=bestLocalPose;
//cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl;
//here we look for the best move;
}while (currentScore>bestScore || refinement<m_optRecursiveIterations);
//cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
//cout << __PRETTY_FUNCTION__ << "iterations=" << c_iterations<< endl;
pnew=currentPose;
return bestScore;
}
struct ScoredMove{
OrientedPoint pose;
double score;
double likelihood;
};
typedef std::list<ScoredMove> ScoredMoveList;
double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
ScoredMoveList moveList;
double bestScore=-1;
OrientedPoint currentPose=init;
ScoredMove sm={currentPose,0,0};
unsigned int matched=likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
double currentScore=sm.score;
moveList.push_back(sm);
double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
unsigned int refinement=0;
enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
do{
if (bestScore>=currentScore){
refinement++;
adelta*=.5;
ldelta*=.5;
}
bestScore=currentScore;
// cout <<"score="<< currentScore << " refinement=" << refinement;
// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
OrientedPoint bestLocalPose=currentPose;
OrientedPoint localPose=currentPose;
Move move=Front;
do {
localPose=currentPose;
switch(move){
case Front:
localPose.x+=ldelta;
move=Back;
break;
case Back:
localPose.x-=ldelta;
move=Left;
break;
case Left:
localPose.y-=ldelta;
move=Right;
break;
case Right:
localPose.y+=ldelta;
move=TurnLeft;
break;
case TurnLeft:
localPose.theta+=adelta;
move=TurnRight;
break;
case TurnRight:
localPose.theta-=adelta;
move=Done;
break;
default:;
}
double localScore, localLikelihood;
//update the score
matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);
if (localScore>currentScore){
currentScore=localScore;
bestLocalPose=localPose;
}
sm.score=localScore;
sm.likelihood=localLikelihood;
sm.pose=localPose;
moveList.push_back(sm);
//update the move list
} while(move!=Done);
currentPose=bestLocalPose;
//cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl;
//here we look for the best move;
}while (currentScore>bestScore || refinement<m_optRecursiveIterations);
//cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
//normalize the likelihood
double lmin=1e9;
double lmax=-1e9;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
lmin=it->likelihood<lmin?it->likelihood:lmin;
lmax=it->likelihood>lmax?it->likelihood:lmax;
}
//cout << "lmin=" << lmin << " lmax=" << lmax<< endl;
for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
it->likelihood=exp(it->likelihood-lmax);
//cout << "l=" << it->likelihood << endl;
}
//compute the mean
OrientedPoint mean(0,0,0);
double lacc=0;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
mean=mean+it->pose*it->likelihood;
lacc+=it->likelihood;
}
mean=mean*(1./lacc);
//OrientedPoint delta=mean-currentPose;
//cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
OrientedPoint delta=it->pose-mean;
delta.theta=atan2(sin(delta.theta), cos(delta.theta));
cov.xx+=delta.x*delta.x*it->likelihood;
cov.yy+=delta.y*delta.y*it->likelihood;
cov.tt+=delta.theta*delta.theta*it->likelihood;
cov.xy+=delta.x*delta.y*it->likelihood;
cov.xt+=delta.x*delta.theta*it->likelihood;
cov.yt+=delta.y*delta.theta*it->likelihood;
}
cov.xx/=lacc, cov.xy/=lacc, cov.xt/=lacc, cov.yy/=lacc, cov.yt/=lacc, cov.tt/=lacc;
_mean=currentPose;
_cov=cov;
return bestScore;
}
void ScanMatcher::setLaserParameters
(unsigned int beams, double* angles, const OrientedPoint& lpose){
if (m_laserAngles)
delete [] m_laserAngles;
m_laserPose=lpose;
m_laserBeams=beams;
m_laserAngles=new double[beams];
memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);
}
double ScanMatcher::likelihood
(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
ScoredMoveList moveList;
for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
OrientedPoint rp=p;
rp.x+=xx;
rp.y+=yy;
rp.theta+=tt;
ScoredMove sm;
sm.pose=rp;
likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
moveList.push_back(sm);
}
//OrientedPoint delta=mean-currentPose;
//cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
//normalize the likelihood
double lmax=-1e9;
double lcum=0;
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
lmax=it->likelihood>lmax?it->likelihood:lmax;
}
for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
//it->likelihood=exp(it->likelihood-lmax);
lcum+=exp(it->likelihood-lmax);
it->likelihood=exp(it->likelihood-lmax);
//cout << "l=" << it->likelihood << endl;
}
OrientedPoint mean(0,0,0);
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
mean=mean+it->pose*it->likelihood;
}
mean=mean*(1./lcum);
CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
OrientedPoint delta=it->pose-mean;
delta.theta=atan2(sin(delta.theta), cos(delta.theta));
cov.xx+=delta.x*delta.x*it->likelihood;
cov.yy+=delta.y*delta.y*it->likelihood;
cov.tt+=delta.theta*delta.theta*it->likelihood;
cov.xy+=delta.x*delta.y*it->likelihood;
cov.xt+=delta.x*delta.theta*it->likelihood;
cov.yt+=delta.y*delta.theta*it->likelihood;
}
cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
_mean=mean;
_cov=cov;
_lmax=lmax;
return log(lcum)+lmax;
}
void ScanMatcher::setMatchingParameters
(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma, unsigned int likelihoodSkip){
m_usableRange=urange;
m_laserMaxRange=range;
m_kernelSize=kernsize;
m_optLinearDelta=lopt;
m_optAngularDelta=aopt;
m_optRecursiveIterations=iterations;
m_gaussianSigma=sigma;
m_likelihoodSigma=likelihoodSigma;
m_likelihoodSkip=likelihoodSkip;
}
};
slam_gmapping/openslam_gmapping/scanmatcher/scanmatcherprocessor.cpp
+233
View File
@@ -0,0 +1,233 @@
#include <iostream>
#include "scanmatcherprocessor.h"
#include "eig3.h"
//#define SCANMATHCERPROCESSOR_DEBUG
namespace GMapping {
using namespace std;
ScanMatcherProcessor::ScanMatcherProcessor(const ScanMatcherMap& m)
: m_map(m.getCenter(), m.getWorldSizeX(), m.getWorldSizeY(), m.getResolution()),
m_pose(0,0,0){
m_regScore=300;
m_critScore=.5*m_regScore;
m_maxMove=1;
m_beams=0;
m_computeCovariance=false;
//m_eigenspace=gsl_eigen_symmv_alloc(3);
useICP=false;
}
ScanMatcherProcessor::ScanMatcherProcessor
(double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta):
m_map(Point((xmax+xmin)*.5, (ymax+ymin)*.5), xmax-xmin, ymax-ymin, patchdelta), m_pose(0,0,0){
m_regScore=300;
m_critScore=.5*m_regScore;
m_maxMove=1;
m_beams=0;
m_computeCovariance=false;
//m_eigenspace=gsl_eigen_symmv_alloc(3);
useICP=false;
}
ScanMatcherProcessor::~ScanMatcherProcessor (){
//gsl_eigen_symmv_free(m_eigenspace);
}
void ScanMatcherProcessor::setSensorMap(const SensorMap& smap, std::string sensorName){
m_sensorMap=smap;
/*
Construct the angle table for the sensor
FIXME has to be extended to more than one laser...
*/
SensorMap::const_iterator laser_it=m_sensorMap.find(sensorName);
assert(laser_it!=m_sensorMap.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 ScanMatcherProcessor::init(){
m_first=true;
m_pose=OrientedPoint(0,0,0);
m_count=0;
}
void ScanMatcherProcessor::processScan(const RangeReading & reading){
/**retireve the position from the reading, and compute the odometry*/
OrientedPoint relPose=reading.getPose();
if (!m_count){
m_odoPose=relPose;
}
//compute the move in the scan m_matcher
//reference frame
OrientedPoint move=relPose-m_odoPose;
double dth=m_odoPose.theta-m_pose.theta;
// cout << "rel-move x="<< move.x << " y=" << move.y << " theta=" << move.theta << endl;
double lin_move=move*move;
if (lin_move>m_maxMove){
cerr << "Too big jump in the log file: " << lin_move << endl;
cerr << "relPose=" << relPose.x << " " <<relPose.y << endl;
cerr << "ignoring" << endl;
return;
//assert(0);
dth=0;
move.x=move.y=move.theta=0;
}
double s=sin(dth), c=cos(dth);
OrientedPoint dPose;
dPose.x=c*move.x-s*move.y;
dPose.y=s*move.x+c*move.y;
dPose.theta=move.theta;
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << "abs-move x="<< dPose.x << " y=" << dPose.y << " theta=" << dPose.theta << endl;
#endif
m_pose=m_pose+dPose;
m_pose.theta=atan2(sin(m_pose.theta), cos(m_pose.theta));
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << "StartPose: x="
<< m_pose.x << " y=" << m_pose.y << " theta=" << m_pose.theta << endl;
#endif
m_odoPose=relPose; //update the past pose for the next iteration
//FIXME here I assume that everithing is referred to the center of the robot,
//while the offset of the laser has to be taken into account
assert(reading.size()==m_beams);
/*
double * plainReading = new double[m_beams];
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << "PackedReadings ";
#endif
for(unsigned int i=0; i<m_beams; i++){
plainReading[i]=reading[i];
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << plainReading[i] << " ";
#endif
}
*/
double * plainReading = new double[m_beams];
reading.rawView(plainReading, m_map.getDelta());
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << endl;
#endif
//the final stuff: scan match the pose
double score=0;
OrientedPoint newPose=m_pose;
if (m_count){
if(m_computeCovariance){
ScanMatcher::CovarianceMatrix cov;
score=m_matcher.optimize(newPose, cov, m_map, m_pose, plainReading);
/*
gsl_matrix* m=gsl_matrix_alloc(3,3);
gsl_matrix_set(m,0,0,cov.xx); gsl_matrix_set(m,0,1,cov.xy); gsl_matrix_set(m,0,2,cov.xt);
gsl_matrix_set(m,1,0,cov.xy); gsl_matrix_set(m,1,1,cov.yy); gsl_matrix_set(m,1,2,cov.yt);
gsl_matrix_set(m,2,0,cov.xt); gsl_matrix_set(m,2,1,cov.yt); gsl_matrix_set(m,2,2,cov.tt);
gsl_matrix* evec=gsl_matrix_alloc(3,3);
gsl_vector* eval=gsl_vector_alloc(3);
*/
double m[3][3];
double evec[3][3];
double eval[3];
m[0][0] = cov.xx;
m[0][1] = cov.xy;
m[0][2] = cov.xt;
m[1][0] = cov.xy;
m[1][1] = cov.yy;
m[1][2] = cov.yt;
m[2][0] = cov.xt;
m[2][1] = cov.yt;
m[2][2] = cov.tt;
//gsl_eigen_symmv (m, eval, evec, m_eigenspace);
eigen_decomposition(m,evec,eval);
#ifdef SCANMATHCERPROCESSOR_DEBUG
//cout << "evals=" << gsl_vector_get(eval, 0) << " " << gsl_vector_get(eval, 1)<< " " << gsl_vector_get(eval, 2)<<endl;
cout << "evals=" << eval[0] << " " << eval[1]<< " " << eval[2]<<endl;
#endif
//gsl_matrix_free(m);
//gsl_matrix_free(evec);
//gsl_vector_free(eval);
} else {
if (useICP){
cerr << "USING ICP" << endl;
score=m_matcher.icpOptimize(newPose, m_map, m_pose, plainReading);
}else
score=m_matcher.optimize(newPose, m_map, m_pose, plainReading);
}
}
//...and register the scan
if (!m_count || score<m_regScore){
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << "Registering" << endl;
#endif
m_matcher.invalidateActiveArea();
if (score<m_critScore){
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << "New Scan added, using odo pose" << endl;
#endif
m_matcher.registerScan(m_map, m_pose, plainReading);
} else {
m_matcher.registerScan(m_map, newPose, plainReading);
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << "New Scan added, using matched pose" << endl;
#endif
}
}
#ifdef SCANMATHCERPROCESSOR_DEBUG
cout << " FinalPose: x="
<< newPose.x << " y=" << newPose.y << " theta=" << newPose.theta << endl;
cout << "score=" << score << endl;
#endif
m_pose=newPose;
delete [] plainReading;
m_count++;
}
void ScanMatcherProcessor::setMatchingParameters
(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance){
m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations);
m_computeCovariance=computeCovariance;
}
void ScanMatcherProcessor::setRegistrationParameters(double regScore, double critScore){
m_regScore=regScore;
m_critScore=critScore;
}
OrientedPoint ScanMatcherProcessor::getPose() const{
return m_pose;
}
};
slam_gmapping/openslam_gmapping/scanmatcher/scanmatcherprocessor.h
+48
View File
@@ -0,0 +1,48 @@
#ifndef SCANMATCHERPROCESSOR_H
#define SCANMATCHERPROCESSOR_H
#include <gmapping/log/sensorlog.h>
#include <gmapping/sensor/sensor_range/rangesensor.h>
#include <gmapping/sensor/sensor_range/rangereading.h>
//#include <gsl/gsl_eigen.h>
#include <gmapping/scanmatcher/scanmatcher.h>
namespace GMapping {
class ScanMatcherProcessor{
public:
ScanMatcherProcessor(const ScanMatcherMap& m);
ScanMatcherProcessor (double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta);
virtual ~ScanMatcherProcessor ();
virtual void processScan(const RangeReading & reading);
void setSensorMap(const SensorMap& smap, std::string sensorName="FLASER");
void init();
void setMatchingParameters
(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance=false);
void setRegistrationParameters(double regScore, double critScore);
OrientedPoint getPose() const;
inline const ScanMatcherMap& getMap() const {return m_map;}
inline ScanMatcher& matcher() {return m_matcher;}
inline void setmaxMove(double mmove){m_maxMove=mmove;}
bool useICP;
protected:
ScanMatcher m_matcher;
bool m_computeCovariance;
bool m_first;
SensorMap m_sensorMap;
double m_regScore, m_critScore;
unsigned int m_beams;
double m_maxMove;
//state
ScanMatcherMap m_map;
OrientedPoint m_pose;
OrientedPoint m_odoPose;
int m_count;
//gsl_eigen_symmv_workspace * m_eigenspace;
};
};
#endif
slam_gmapping/openslam_gmapping/scanmatcher/smmap.cpp
+15
View File
@@ -0,0 +1,15 @@
#include <gmapping/scanmatcher/smmap.h>
namespace GMapping {
const PointAccumulator& PointAccumulator::Unknown(){
if (! unknown_ptr)
unknown_ptr=new PointAccumulator;
return *unknown_ptr;
}
PointAccumulator* PointAccumulator::unknown_ptr=0;
};