Fawkes API  Fawkes Development Version
amcl_odom.cpp
00001 /***************************************************************************
00002  *  amcl_odom.cpp: AMCL odometry routines
00003  *
00004  *  Created: Thu May 24 18:51:42 2012
00005  *  Copyright  2000  Brian Gerkey
00006  *             2000  Kasper Stoy
00007  *             2012  Tim Niemueller [www.niemueller.de]
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 /*  From:
00024  *  Player - One Hell of a Robot Server (LGPL)
00025  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00026  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00027  */
00028 ///////////////////////////////////////////////////////////////////////////
00029 // Desc: AMCL odometry routines
00030 // Author: Andrew Howard
00031 // Date: 6 Feb 2003
00032 ///////////////////////////////////////////////////////////////////////////
00033 
00034 #include <algorithm>
00035 
00036 #include <sys/types.h> // required by Darwin
00037 #include <math.h>
00038 
00039 #include "amcl_odom.h"
00040 
00041 using namespace amcl;
00042 
00043 /// @cond EXTERNAL
00044 
00045 static double
00046 normalize(double z)
00047 {
00048   return atan2(sin(z),cos(z));
00049 }
00050 static double
00051 angle_diff(double a, double b)
00052 {
00053   double d1, d2;
00054   a = normalize(a);
00055   b = normalize(b);
00056   d1 = a-b;
00057   d2 = 2*M_PI - fabs(d1);
00058   if(d1 > 0)
00059     d2 *= -1.0;
00060   if(fabs(d1) < fabs(d2))
00061     return(d1);
00062   else
00063     return(d2);
00064 }
00065 
00066 ////////////////////////////////////////////////////////////////////////////////
00067 // Default constructor
00068 AMCLOdom::AMCLOdom() : AMCLSensor()
00069 {
00070   this->time = 0.0;
00071 }
00072 
00073 void
00074 AMCLOdom::SetModelDiff(double alpha1, 
00075                        double alpha2, 
00076                        double alpha3, 
00077                        double alpha4)
00078 {
00079   this->model_type = ODOM_MODEL_DIFF;
00080   this->alpha1 = alpha1;
00081   this->alpha2 = alpha2;
00082   this->alpha3 = alpha3;
00083   this->alpha4 = alpha4;
00084 }
00085 
00086 void
00087 AMCLOdom::SetModelOmni(double alpha1, 
00088                        double alpha2, 
00089                        double alpha3, 
00090                        double alpha4,
00091                        double alpha5)
00092 {
00093   this->model_type = ODOM_MODEL_OMNI;
00094   this->alpha1 = alpha1;
00095   this->alpha2 = alpha2;
00096   this->alpha3 = alpha3;
00097   this->alpha4 = alpha4;
00098   this->alpha5 = alpha5;
00099 }
00100 
00101 
00102 ////////////////////////////////////////////////////////////////////////////////
00103 // Apply the action model
00104 bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
00105 {
00106   AMCLOdomData *ndata;
00107   ndata = (AMCLOdomData*) data;
00108 
00109   // Compute the new sample poses
00110   pf_sample_set_t *set;
00111 
00112   set = pf->sets + pf->current_set;
00113   pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
00114 
00115   if(this->model_type == ODOM_MODEL_OMNI)
00116   {
00117     double delta_trans, delta_rot, delta_bearing;
00118     double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
00119 
00120     delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
00121                        ndata->delta.v[1]*ndata->delta.v[1]);
00122     delta_rot = ndata->delta.v[2];
00123 
00124     // Precompute a couple of things
00125     double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) +
00126                                alpha1 * (delta_rot*delta_rot));
00127     double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) +
00128                              alpha2 * (delta_trans*delta_trans));
00129     double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) +
00130                                 alpha5 * (delta_trans*delta_trans));
00131 
00132     for (int i = 0; i < set->sample_count; i++)
00133     {
00134       pf_sample_t* sample = set->samples + i;
00135 
00136       delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
00137                                  old_pose.v[2]) + sample->pose.v[2];
00138       double cs_bearing = cos(delta_bearing);
00139       double sn_bearing = sin(delta_bearing);
00140 
00141       // Sample pose differences
00142       delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
00143       delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
00144       delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
00145       // Apply sampled update to particle pose
00146       sample->pose.v[0] += (delta_trans_hat * cs_bearing + 
00147                             delta_strafe_hat * sn_bearing);
00148       sample->pose.v[1] += (delta_trans_hat * sn_bearing - 
00149                             delta_strafe_hat * cs_bearing);
00150       sample->pose.v[2] += delta_rot_hat ;
00151       sample->weight = 1.0 / set->sample_count;
00152     }
00153   }
00154   else //(this->model_type == ODOM_MODEL_DIFF)
00155   {
00156     // Implement sample_motion_odometry (Prob Rob p 136)
00157     double delta_rot1, delta_trans, delta_rot2;
00158     double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
00159     double delta_rot1_noise, delta_rot2_noise;
00160 
00161     // Avoid computing a bearing from two poses that are extremely near each
00162     // other (happens on in-place rotation).
00163     if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + 
00164             ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
00165       delta_rot1 = 0.0;
00166     else
00167       delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
00168                               old_pose.v[2]);
00169     delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
00170                        ndata->delta.v[1]*ndata->delta.v[1]);
00171     delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
00172 
00173     // We want to treat backward and forward motion symmetrically for the
00174     // noise model to be applied below.  The standard model seems to assume
00175     // forward motion.
00176     delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
00177                                 fabs(angle_diff(delta_rot1,M_PI)));
00178     delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
00179                                 fabs(angle_diff(delta_rot2,M_PI)));
00180 
00181     for (int i = 0; i < set->sample_count; i++)
00182     {
00183       pf_sample_t* sample = set->samples + i;
00184 
00185       // Sample pose differences
00186       delta_rot1_hat = angle_diff(delta_rot1,
00187                                   pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
00188                                                   this->alpha2*delta_trans*delta_trans));
00189       delta_trans_hat = delta_trans - 
00190               pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
00191                               this->alpha4*delta_rot1_noise*delta_rot1_noise +
00192                               this->alpha4*delta_rot2_noise*delta_rot2_noise);
00193       delta_rot2_hat = angle_diff(delta_rot2,
00194                                   pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
00195                                                   this->alpha2*delta_trans*delta_trans));
00196 
00197       // Apply sampled update to particle pose
00198       sample->pose.v[0] += delta_trans_hat * 
00199               cos(sample->pose.v[2] + delta_rot1_hat);
00200       sample->pose.v[1] += delta_trans_hat * 
00201               sin(sample->pose.v[2] + delta_rot1_hat);
00202       sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
00203       sample->weight = 1.0 / set->sample_count;
00204     }
00205   }
00206 
00207   return true;
00208 }
00209 
00210 /// @endcond
00211