19 m_optRecursiveIterations=3;
25 m_lasamplerange=0.005;
28 m_fullnessThreshold=0.1;
29 m_angularOdometryReliability=0.;
30 m_linearOdometryReliability=0.;
31 m_freeCellRatio=sqrt(2.);
120 lp.
x+=cos(p.
theta)*m_laserPose.x-sin(p.
theta)*m_laserPose.y;
121 lp.
y+=sin(p.
theta)*m_laserPose.x+cos(p.
theta)*m_laserPose.y;
122 lp.
theta+=m_laserPose.theta;
135 for (
const double* r=readings+m_initialBeamsSkip; r<readings+
m_laserBeams; r++, angle++){
136 if (*r>m_laserMaxRange||*r==0.0||isnan(*r))
continue;
137 double d=*r>m_usableRange?m_usableRange:*r;
139 phit.
x+=d*cos(lp.
theta+*angle);
140 phit.
y+=d*sin(lp.
theta+*angle);
154 min.x=(
min.x >= lmin.
x )? lmin.
x:
min.x-m_enlargeStep;
155 max.x=(
max.x <= lmax.
x )? lmax.
x:
max.x+m_enlargeStep;
156 min.y=(
min.y >= lmin.
y )? lmin.
y:
min.y-m_enlargeStep;
157 max.y=(
max.y <= lmax.
y )? lmax.
y:
max.y+m_enlargeStep;
165 for (
const double* r=readings+m_initialBeamsSkip; r<readings+
m_laserBeams; r++, angle++)
168 if (d>m_laserMaxRange||d==0.0||isnan(d))
185 if (d<m_usableRange){
187 assert(cp.
x>=0 && cp.
y>=0);
188 activeArea.insert(cp);
191 if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r))
continue;
193 phit.
x+=*r*cos(lp.
theta+*angle);
194 phit.
y+=*r*sin(lp.
theta+*angle);
196 assert(p1.
x>=0 && p1.
y>=0);
198 assert(cp.
x>=0 && cp.
y>=0);
199 activeArea.insert(cp);
223 lp.
x+=cos(p.
theta)*m_laserPose.x-sin(p.
theta)*m_laserPose.y;
224 lp.
y+=sin(p.
theta)*m_laserPose.x+cos(p.
theta)*m_laserPose.y;
225 lp.
theta+=m_laserPose.theta;
231 for (
const double* r=readings+m_initialBeamsSkip; r<readings+
m_laserBeams; r++, angle++)
234 if (d>m_laserMaxRange||d==0.0||isnan(d))
251 if (d<m_usableRange){
258 if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r))
continue;
260 phit.
x+=*r*cos(lp.
theta+*angle);
261 phit.
y+=*r*sin(lp.
theta+*angle);
263 assert(p1.
x>=0 && p1.
y>=0);
321 double sc=
score(map, init, readings);;
327 sc=
icpStep(pnew, map, start, readings);
332 }
while (sc>currentScore);
333 cerr <<
"i="<< iterations << endl;
340 double currentScore=
score(map, currentPose, readings);
341 double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
342 unsigned int refinement=0;
343 enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
351 if (bestScore>=currentScore){
356 bestScore=currentScore;
364 localPose=currentPose;
383 localPose.
theta+=adelta;
387 localPose.
theta-=adelta;
394 if (m_angularOdometryReliability>0.){
395 double dth=init.
theta-localPose.
theta; dth=atan2(sin(dth), cos(dth)); dth*=dth;
396 odo_gain*=exp(-m_angularOdometryReliability*dth);
398 if (m_linearOdometryReliability>0.){
399 double dx=init.
x-localPose.
x;
400 double dy=init.
y-localPose.
y;
401 double drho=dx*dx+dy*dy;
402 odo_gain*=exp(-m_linearOdometryReliability*drho);
404 double localScore=odo_gain*
score(map, localPose, readings);
406 if (localScore>currentScore){
407 currentScore=localScore;
408 bestLocalPose=localPose;
412 currentPose=bestLocalPose;
415 }
while (currentScore>bestScore || refinement<m_optRecursiveIterations);
436 double currentScore=sm.
score;
437 moveList.push_back(sm);
438 double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
439 unsigned int refinement=0;
441 enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
443 if (bestScore>=currentScore){
448 bestScore=currentScore;
456 localPose=currentPose;
475 localPose.
theta+=adelta;
479 localPose.
theta-=adelta;
484 double localScore, localLikelihood;
487 if (m_angularOdometryReliability>0.){
488 double dth=init.
theta-localPose.
theta; dth=atan2(sin(dth), cos(dth)); dth*=dth;
489 odo_gain*=exp(-m_angularOdometryReliability*dth);
491 if (m_linearOdometryReliability>0.){
492 double dx=init.
x-localPose.
x;
493 double dy=init.
y-localPose.
y;
494 double drho=dx*dx+dy*dy;
495 odo_gain*=exp(-m_linearOdometryReliability*drho);
497 localScore=odo_gain*
score(map, localPose, readings);
501 if (localScore>currentScore){
502 currentScore=localScore;
503 bestLocalPose=localPose;
508 moveList.push_back(sm);
511 currentPose=bestLocalPose;
514 }
while (currentScore>bestScore || refinement<m_optRecursiveIterations);
521 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
522 lmin=it->likelihood<lmin?it->likelihood:lmin;
523 lmax=it->likelihood>lmax?it->likelihood:lmax;
526 for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
527 it->likelihood=exp(it->likelihood-lmax);
533 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
534 mean=mean+it->pose*it->likelihood;
535 lacc+=it->likelihood;
541 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
546 cov.tt+=
delta.theta*
delta.theta*it->likelihood;
551 cov.xx/=lacc, cov.xy/=lacc, cov.xt/=lacc, cov.yy/=lacc, cov.yt/=lacc, cov.tt/=lacc;
567 memcpy(m_laserAngles, angles,
sizeof(
double)*m_laserBeams);
575 for (
double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
576 for (
double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
577 for (
double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
588 moveList.push_back(sm);
596 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
597 lmax=it->likelihood>lmax?it->likelihood:lmax;
599 for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
601 lcum+=exp(it->likelihood-lmax);
602 it->likelihood=exp(it->likelihood-lmax);
608 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
609 mean=mean+it->pose*it->likelihood;
610 s+=it->likelihood*sin(it->pose.theta);
611 c+=it->likelihood*cos(it->pose.theta);
620 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
630 cov.
xx/=lcum, cov.
xy/=lcum, cov.
xt/=lcum, cov.
yy/=lcum, cov.
yt/=lcum, cov.
tt/=lcum;
635 return log(lcum)+lmax;
640 Gaussian3& odometry,
const double* readings,
double gain){
644 for (
double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
645 for (
double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
646 for (
double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
659 moveList.push_back(sm);
667 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
668 lmax=it->likelihood>lmax?it->likelihood:lmax;
670 for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
672 lcum+=exp(it->likelihood-lmax);
673 it->likelihood=exp(it->likelihood-lmax);
679 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
680 mean=mean+it->pose*it->likelihood;
681 s+=it->likelihood*sin(it->pose.theta);
682 c+=it->likelihood*cos(it->pose.theta);
691 for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
701 cov.
xx/=lcum, cov.
xy/=lcum, cov.
xt/=lcum, cov.
yy/=lcum, cov.
yt/=lcum, cov.
tt/=lcum;
706 double v=log(lcum)+lmax;
712 (
double urange,
double range,
double sigma,
int kernsize,
double lopt,
double aopt,
int iterations,
double likelihoodSigma,
unsigned int likelihoodSkip){
713 m_usableRange=urange;
714 m_laserMaxRange=range;
715 m_kernelSize=kernsize;
716 m_optLinearDelta=lopt;
717 m_optAngularDelta=aopt;
718 m_optRecursiveIterations=iterations;
719 m_gaussianSigma=sigma;
720 m_likelihoodSigma=likelihoodSigma;
721 m_likelihoodSkip=likelihoodSkip;