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 "amcl_odom.h"
35 
36 #include <sys/types.h> // required by Darwin
37 
38 #include <algorithm>
39 #include <math.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  this->model_type = ODOM_MODEL_OMNI;
73  this->alpha1 = 5;
74  this->alpha2 = 5;
75  this->alpha3 = .6;
76  this->alpha4 = .6;
77  this->alpha5 = .6;
78 }
79 
80 void
81 AMCLOdom::SetModelDiff(double alpha1, double alpha2, double alpha3, double alpha4)
82 {
83  this->model_type = ODOM_MODEL_DIFF;
84  this->alpha1 = alpha1;
85  this->alpha2 = alpha2;
86  this->alpha3 = alpha3;
87  this->alpha4 = alpha4;
88 }
89 
90 void
91 AMCLOdom::SetModelOmni(double alpha1, double alpha2, double alpha3, double alpha4, 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 // Apply the action model
103 bool
104 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  double delta_trans =
117  sqrt(ndata->delta.v[0] * ndata->delta.v[0] + ndata->delta.v[1] * ndata->delta.v[1]);
118  double delta_rot = ndata->delta.v[2];
119 
120  // Precompute a couple of things
121  double trans_hat_stddev =
122  (alpha3 * (delta_trans * delta_trans) + alpha1 * (delta_rot * delta_rot));
123  double rot_hat_stddev =
124  (alpha4 * (delta_rot * delta_rot) + alpha2 * (delta_trans * delta_trans));
125  double strafe_hat_stddev =
126  (alpha1 * (delta_rot * delta_rot) + alpha5 * (delta_trans * delta_trans));
127 
128  for (int i = 0; i < set->sample_count; i++) {
129  pf_sample_t *sample = set->samples + i;
130 
131  double delta_bearing =
132  angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), old_pose.v[2]) + sample->pose.v[2];
133  double cs_bearing = cos(delta_bearing);
134  double sn_bearing = sin(delta_bearing);
135 
136  // Sample pose differences
137  double delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
138  double delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
139  double delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
140  // Apply sampled update to particle pose
141  sample->pose.v[0] += (delta_trans_hat * cs_bearing + delta_strafe_hat * sn_bearing);
142  sample->pose.v[1] += (delta_trans_hat * sn_bearing - delta_strafe_hat * cs_bearing);
143  sample->pose.v[2] += delta_rot_hat;
144  sample->weight = 1.0 / set->sample_count;
145  }
146  } else //(this->model_type == ODOM_MODEL_DIFF)
147  {
148  // Implement sample_motion_odometry (Prob Rob p 136)
149  double delta_rot1, delta_trans, delta_rot2;
150  double delta_rot1_noise, delta_rot2_noise;
151 
152  // Avoid computing a bearing from two poses that are extremely near each
153  // other (happens on in-place rotation).
154  if (sqrt(ndata->delta.v[1] * ndata->delta.v[1] + ndata->delta.v[0] * ndata->delta.v[0]) < 0.01)
155  delta_rot1 = 0.0;
156  else
157  delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), old_pose.v[2]);
158  delta_trans =
159  sqrt(ndata->delta.v[0] * ndata->delta.v[0] + ndata->delta.v[1] * ndata->delta.v[1]);
160  delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
161 
162  // We want to treat backward and forward motion symmetrically for the
163  // noise model to be applied below. The standard model seems to assume
164  // forward motion.
165  delta_rot1_noise =
166  std::min(fabs(angle_diff(delta_rot1, 0.0)), fabs(angle_diff(delta_rot1, M_PI)));
167  delta_rot2_noise =
168  std::min(fabs(angle_diff(delta_rot2, 0.0)), fabs(angle_diff(delta_rot2, M_PI)));
169 
170  for (int i = 0; i < set->sample_count; i++) {
171  pf_sample_t *sample = set->samples + i;
172 
173  // Sample pose differences
174  double delta_rot1_hat =
175  angle_diff(delta_rot1,
176  pf_ran_gaussian(this->alpha1 * delta_rot1_noise * delta_rot1_noise
177  + this->alpha2 * delta_trans * delta_trans));
178  double delta_trans_hat =
179  delta_trans
180  - pf_ran_gaussian(this->alpha3 * delta_trans * delta_trans
181  + this->alpha4 * delta_rot1_noise * delta_rot1_noise
182  + this->alpha4 * delta_rot2_noise * delta_rot2_noise);
183  double delta_rot2_hat =
184  angle_diff(delta_rot2,
185  pf_ran_gaussian(this->alpha1 * delta_rot2_noise * delta_rot2_noise
186  + this->alpha2 * delta_trans * delta_trans));
187 
188  // Apply sampled update to particle pose
189  sample->pose.v[0] += delta_trans_hat * cos(sample->pose.v[2] + delta_rot1_hat);
190  sample->pose.v[1] += delta_trans_hat * sin(sample->pose.v[2] + delta_rot1_hat);
191  sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
192  sample->weight = 1.0 / set->sample_count;
193  }
194  }
195 
196  return true;
197 }
198 
199 /// @endcond