Fawkes API
Fawkes Development Version
|
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