Fawkes API  Fawkes Development Version
amcl_odom.cpp
1 /***************************************************************************
2  * amcl_odom.cpp: AMCL odometry routines
3  *
4  * Created: Thu May 24 18:51:42 2012
5  * Copyright 2000 Brian Gerkey
6  * 2000 Kasper Stoy
7  * 2012 Tim Niemueller [www.niemueller.de]
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 /* From:
24  * Player - One Hell of a Robot Server (LGPL)
25  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
26  * gerkey@usc.edu kaspers@robotics.usc.edu
27  */
28 ///////////////////////////////////////////////////////////////////////////
29 // Desc: AMCL odometry routines
30 // Author: Andrew Howard
31 // Date: 6 Feb 2003
32 ///////////////////////////////////////////////////////////////////////////
33 
34 #include <algorithm>
35 
36 #include <sys/types.h> // required by Darwin
37 #include <math.h>
38 
39 #include "amcl_odom.h"
40 
41 using namespace amcl;
42 
43 /// @cond EXTERNAL
44 
45 static double
46 normalize(double z)
47 {
48  return atan2(sin(z),cos(z));
49 }
50 static double
51 angle_diff(double a, double b)
52 {
53  double d1, d2;
54  a = normalize(a);
55  b = normalize(b);
56  d1 = a-b;
57  d2 = 2*M_PI - fabs(d1);
58  if(d1 > 0)
59  d2 *= -1.0;
60  if(fabs(d1) < fabs(d2))
61  return(d1);
62  else
63  return(d2);
64 }
65 
66 ////////////////////////////////////////////////////////////////////////////////
67 // Default constructor
68 AMCLOdom::AMCLOdom() : AMCLSensor()
69 {
70  this->time = 0.0;
71 }
72 
73 void
74 AMCLOdom::SetModelDiff(double alpha1,
75  double alpha2,
76  double alpha3,
77  double alpha4)
78 {
79  this->model_type = ODOM_MODEL_DIFF;
80  this->alpha1 = alpha1;
81  this->alpha2 = alpha2;
82  this->alpha3 = alpha3;
83  this->alpha4 = alpha4;
84 }
85 
86 void
87 AMCLOdom::SetModelOmni(double alpha1,
88  double alpha2,
89  double alpha3,
90  double alpha4,
91  double alpha5)
92 {
93  this->model_type = ODOM_MODEL_OMNI;
94  this->alpha1 = alpha1;
95  this->alpha2 = alpha2;
96  this->alpha3 = alpha3;
97  this->alpha4 = alpha4;
98  this->alpha5 = alpha5;
99 }
100 
101 
102 ////////////////////////////////////////////////////////////////////////////////
103 // Apply the action model
104 bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
105 {
106  AMCLOdomData *ndata;
107  ndata = (AMCLOdomData*) data;
108 
109  // Compute the new sample poses
110  pf_sample_set_t *set;
111 
112  set = pf->sets + pf->current_set;
113  pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
114 
115  if(this->model_type == ODOM_MODEL_OMNI)
116  {
117  double delta_trans, delta_rot, delta_bearing;
118  double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
119 
120  delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
121  ndata->delta.v[1]*ndata->delta.v[1]);
122  delta_rot = ndata->delta.v[2];
123 
124  // Precompute a couple of things
125  double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) +
126  alpha1 * (delta_rot*delta_rot));
127  double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) +
128  alpha2 * (delta_trans*delta_trans));
129  double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) +
130  alpha5 * (delta_trans*delta_trans));
131 
132  for (int i = 0; i < set->sample_count; i++)
133  {
134  pf_sample_t* sample = set->samples + i;
135 
136  delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
137  old_pose.v[2]) + sample->pose.v[2];
138  double cs_bearing = cos(delta_bearing);
139  double sn_bearing = sin(delta_bearing);
140 
141  // Sample pose differences
142  delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
143  delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
144  delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
145  // Apply sampled update to particle pose
146  sample->pose.v[0] += (delta_trans_hat * cs_bearing +
147  delta_strafe_hat * sn_bearing);
148  sample->pose.v[1] += (delta_trans_hat * sn_bearing -
149  delta_strafe_hat * cs_bearing);
150  sample->pose.v[2] += delta_rot_hat ;
151  sample->weight = 1.0 / set->sample_count;
152  }
153  }
154  else //(this->model_type == ODOM_MODEL_DIFF)
155  {
156  // Implement sample_motion_odometry (Prob Rob p 136)
157  double delta_rot1, delta_trans, delta_rot2;
158  double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
159  double delta_rot1_noise, delta_rot2_noise;
160 
161  // Avoid computing a bearing from two poses that are extremely near each
162  // other (happens on in-place rotation).
163  if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
164  ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
165  delta_rot1 = 0.0;
166  else
167  delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
168  old_pose.v[2]);
169  delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
170  ndata->delta.v[1]*ndata->delta.v[1]);
171  delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
172 
173  // We want to treat backward and forward motion symmetrically for the
174  // noise model to be applied below. The standard model seems to assume
175  // forward motion.
176  delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
177  fabs(angle_diff(delta_rot1,M_PI)));
178  delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
179  fabs(angle_diff(delta_rot2,M_PI)));
180 
181  for (int i = 0; i < set->sample_count; i++)
182  {
183  pf_sample_t* sample = set->samples + i;
184 
185  // Sample pose differences
186  delta_rot1_hat = angle_diff(delta_rot1,
187  pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
188  this->alpha2*delta_trans*delta_trans));
189  delta_trans_hat = delta_trans -
190  pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
191  this->alpha4*delta_rot1_noise*delta_rot1_noise +
192  this->alpha4*delta_rot2_noise*delta_rot2_noise);
193  delta_rot2_hat = angle_diff(delta_rot2,
194  pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
195  this->alpha2*delta_trans*delta_trans));
196 
197  // Apply sampled update to particle pose
198  sample->pose.v[0] += delta_trans_hat *
199  cos(sample->pose.v[2] + delta_rot1_hat);
200  sample->pose.v[1] += delta_trans_hat *
201  sin(sample->pose.v[2] + delta_rot1_hat);
202  sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
203  sample->weight = 1.0 / set->sample_count;
204  }
205  }
206 
207  return true;
208 }
209 
210 /// @endcond
211