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
41using namespace amcl;
42
43/// @cond EXTERNAL
44
45static double
46normalize(double z)
47{
48 return atan2(sin(z), cos(z));
49}
50static double
51angle_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
68AMCLOdom::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
80void
81AMCLOdom::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
90void
91AMCLOdom::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
103bool
104AMCLOdom::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