amcl_odom.cpp
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This library is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU Lesser General Public
00008  *  License as published by the Free Software Foundation; either
00009  *  version 2.1 of the License, or (at your option) any later version.
00010  *
00011  *  This library is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  *  Lesser General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU Lesser General Public
00017  *  License along with this library; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00022 //
00023 // Desc: AMCL odometry routines
00024 // Author: Andrew Howard
00025 // Date: 6 Feb 2003
00026 // CVS: $Id: amcl_odom.cc 7057 2008-10-02 00:44:06Z gbiggs $
00027 //
00029 
00030 #include <algorithm>
00031 
00032 #include <sys/types.h> // required by Darwin
00033 #include <math.h>
00034 
00035 #include "amcl_odom.h"
00036 
00037 using namespace amcl;
00038 
00039 static double
00040 normalize(double z)
00041 {
00042   return atan2(sin(z),cos(z));
00043 }
00044 static double
00045 angle_diff(double a, double b)
00046 {
00047   double d1, d2;
00048   a = normalize(a);
00049   b = normalize(b);
00050   d1 = a-b;
00051   d2 = 2*M_PI - fabs(d1);
00052   if(d1 > 0)
00053     d2 *= -1.0;
00054   if(fabs(d1) < fabs(d2))
00055     return(d1);
00056   else
00057     return(d2);
00058 }
00059 
00061 // Default constructor
00062 AMCLOdom::AMCLOdom() : AMCLSensor()
00063 {
00064   this->time = 0.0;
00065 }
00066 
00067 void
00068 AMCLOdom::SetModelDiff(double alpha1, 
00069                        double alpha2, 
00070                        double alpha3, 
00071                        double alpha4)
00072 {
00073   this->model_type = ODOM_MODEL_DIFF;
00074   this->alpha1 = alpha1;
00075   this->alpha2 = alpha2;
00076   this->alpha3 = alpha3;
00077   this->alpha4 = alpha4;
00078 }
00079 
00080 void
00081 AMCLOdom::SetModelOmni(double alpha1, 
00082                        double alpha2, 
00083                        double alpha3, 
00084                        double alpha4,
00085                        double alpha5)
00086 {
00087   this->model_type = ODOM_MODEL_OMNI;
00088   this->alpha1 = alpha1;
00089   this->alpha2 = alpha2;
00090   this->alpha3 = alpha3;
00091   this->alpha4 = alpha4;
00092   this->alpha5 = alpha5;
00093 }
00094 
00095 void
00096 AMCLOdom::SetModel( odom_model_t type,
00097                     double alpha1,
00098                     double alpha2,
00099                     double alpha3,
00100                     double alpha4,
00101                     double alpha5 )
00102 {
00103   this->model_type = type;
00104   this->alpha1 = alpha1;
00105   this->alpha2 = alpha2;
00106   this->alpha3 = alpha3;
00107   this->alpha4 = alpha4;
00108   this->alpha5 = alpha5;
00109 }
00110 
00112 // Apply the action model
00113 bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
00114 {
00115   AMCLOdomData *ndata;
00116   ndata = (AMCLOdomData*) data;
00117 
00118   // Compute the new sample poses
00119   pf_sample_set_t *set;
00120 
00121   set = pf->sets + pf->current_set;
00122   pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
00123 
00124   switch( this->model_type )
00125   {
00126   case ODOM_MODEL_OMNI:
00127   {
00128     double delta_trans, delta_rot, delta_bearing;
00129     double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
00130 
00131     delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
00132                        ndata->delta.v[1]*ndata->delta.v[1]);
00133     delta_rot = ndata->delta.v[2];
00134 
00135     // Precompute a couple of things
00136     double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) +
00137                                alpha1 * (delta_rot*delta_rot));
00138     double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) +
00139                              alpha2 * (delta_trans*delta_trans));
00140     double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) +
00141                                 alpha5 * (delta_trans*delta_trans));
00142 
00143     for (int i = 0; i < set->sample_count; i++)
00144     {
00145       pf_sample_t* sample = set->samples + i;
00146 
00147       delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
00148                                  old_pose.v[2]) + sample->pose.v[2];
00149       double cs_bearing = cos(delta_bearing);
00150       double sn_bearing = sin(delta_bearing);
00151 
00152       // Sample pose differences
00153       delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
00154       delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
00155       delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
00156       // Apply sampled update to particle pose
00157       sample->pose.v[0] += (delta_trans_hat * cs_bearing + 
00158                             delta_strafe_hat * sn_bearing);
00159       sample->pose.v[1] += (delta_trans_hat * sn_bearing - 
00160                             delta_strafe_hat * cs_bearing);
00161       sample->pose.v[2] += delta_rot_hat ;
00162     }
00163   }
00164   break;
00165   case ODOM_MODEL_DIFF:
00166   {
00167     // Implement sample_motion_odometry (Prob Rob p 136)
00168     double delta_rot1, delta_trans, delta_rot2;
00169     double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
00170     double delta_rot1_noise, delta_rot2_noise;
00171 
00172     // Avoid computing a bearing from two poses that are extremely near each
00173     // other (happens on in-place rotation).
00174     if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 
00175             ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
00176       delta_rot1 = 0.0;
00177     else
00178       delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
00179                               old_pose.v[2]);
00180     delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
00181                        ndata->delta.v[1]*ndata->delta.v[1]);
00182     delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
00183 
00184     // We want to treat backward and forward motion symmetrically for the
00185     // noise model to be applied below.  The standard model seems to assume
00186     // forward motion.
00187     delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
00188                                 fabs(angle_diff(delta_rot1,M_PI)));
00189     delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
00190                                 fabs(angle_diff(delta_rot2,M_PI)));
00191 
00192     for (int i = 0; i < set->sample_count; i++)
00193     {
00194       pf_sample_t* sample = set->samples + i;
00195 
00196       // Sample pose differences
00197       delta_rot1_hat = angle_diff(delta_rot1,
00198                                   pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
00199                                                   this->alpha2*delta_trans*delta_trans));
00200       delta_trans_hat = delta_trans - 
00201               pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
00202                               this->alpha4*delta_rot1_noise*delta_rot1_noise +
00203                               this->alpha4*delta_rot2_noise*delta_rot2_noise);
00204       delta_rot2_hat = angle_diff(delta_rot2,
00205                                   pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
00206                                                   this->alpha2*delta_trans*delta_trans));
00207 
00208       // Apply sampled update to particle pose
00209       sample->pose.v[0] += delta_trans_hat * 
00210               cos(sample->pose.v[2] + delta_rot1_hat);
00211       sample->pose.v[1] += delta_trans_hat * 
00212               sin(sample->pose.v[2] + delta_rot1_hat);
00213       sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
00214     }
00215   }
00216   break;
00217   case ODOM_MODEL_OMNI_CORRECTED:
00218   {
00219     double delta_trans, delta_rot, delta_bearing;
00220     double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
00221 
00222     delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
00223                        ndata->delta.v[1]*ndata->delta.v[1]);
00224     delta_rot = ndata->delta.v[2];
00225 
00226     // Precompute a couple of things
00227     double trans_hat_stddev = sqrt( alpha3 * (delta_trans*delta_trans) +
00228                                     alpha1 * (delta_rot*delta_rot) );
00229     double rot_hat_stddev = sqrt( alpha4 * (delta_rot*delta_rot) +
00230                                   alpha2 * (delta_trans*delta_trans) );
00231     double strafe_hat_stddev = sqrt( alpha1 * (delta_rot*delta_rot) +
00232                                      alpha5 * (delta_trans*delta_trans) );
00233 
00234     for (int i = 0; i < set->sample_count; i++)
00235     {
00236       pf_sample_t* sample = set->samples + i;
00237 
00238       delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
00239                                  old_pose.v[2]) + sample->pose.v[2];
00240       double cs_bearing = cos(delta_bearing);
00241       double sn_bearing = sin(delta_bearing);
00242 
00243       // Sample pose differences
00244       delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
00245       delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
00246       delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
00247       // Apply sampled update to particle pose
00248       sample->pose.v[0] += (delta_trans_hat * cs_bearing + 
00249                             delta_strafe_hat * sn_bearing);
00250       sample->pose.v[1] += (delta_trans_hat * sn_bearing - 
00251                             delta_strafe_hat * cs_bearing);
00252       sample->pose.v[2] += delta_rot_hat ;
00253     }
00254   }
00255   break;
00256   case ODOM_MODEL_DIFF_CORRECTED:
00257   {
00258     // Implement sample_motion_odometry (Prob Rob p 136)
00259     double delta_rot1, delta_trans, delta_rot2;
00260     double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
00261     double delta_rot1_noise, delta_rot2_noise;
00262 
00263     // Avoid computing a bearing from two poses that are extremely near each
00264     // other (happens on in-place rotation).
00265     if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 
00266             ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
00267       delta_rot1 = 0.0;
00268     else
00269       delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
00270                               old_pose.v[2]);
00271     delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
00272                        ndata->delta.v[1]*ndata->delta.v[1]);
00273     delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
00274 
00275     // We want to treat backward and forward motion symmetrically for the
00276     // noise model to be applied below.  The standard model seems to assume
00277     // forward motion.
00278     delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
00279                                 fabs(angle_diff(delta_rot1,M_PI)));
00280     delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
00281                                 fabs(angle_diff(delta_rot2,M_PI)));
00282 
00283     for (int i = 0; i < set->sample_count; i++)
00284     {
00285       pf_sample_t* sample = set->samples + i;
00286 
00287       // Sample pose differences
00288       delta_rot1_hat = angle_diff(delta_rot1,
00289                                   pf_ran_gaussian(sqrt(this->alpha1*delta_rot1_noise*delta_rot1_noise +
00290                                                        this->alpha2*delta_trans*delta_trans)));
00291       delta_trans_hat = delta_trans - 
00292               pf_ran_gaussian(sqrt(this->alpha3*delta_trans*delta_trans +
00293                                    this->alpha4*delta_rot1_noise*delta_rot1_noise +
00294                                    this->alpha4*delta_rot2_noise*delta_rot2_noise));
00295       delta_rot2_hat = angle_diff(delta_rot2,
00296                                   pf_ran_gaussian(sqrt(this->alpha1*delta_rot2_noise*delta_rot2_noise +
00297                                                        this->alpha2*delta_trans*delta_trans)));
00298 
00299       // Apply sampled update to particle pose
00300       sample->pose.v[0] += delta_trans_hat * 
00301               cos(sample->pose.v[2] + delta_rot1_hat);
00302       sample->pose.v[1] += delta_trans_hat * 
00303               sin(sample->pose.v[2] + delta_rot1_hat);
00304       sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
00305     }
00306   }
00307   break;
00308   }
00309   return true;
00310 }


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:48