test_motion_prediction_model_differential_drive.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019, the mcl_3dl authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <gtest/gtest.h>
31 
33 
34 static const double EPS = 1.0e-6;
35 
36 TEST(TestMotionPredictionModelDifferentialDrive, predictWithoutNoise)
37 {
38  const mcl_3dl::Vec3 relative_trans(0.5, -0.1, 0.1);
39  const mcl_3dl::Quat relative_quat(mcl_3dl::Vec3(0.1, -0.1, 0.3));
40 
41  const mcl_3dl::State6DOF odom_prev(mcl_3dl::Vec3(1.0, 1.1, 1.2), mcl_3dl::Vec3(0.3, 0.2, 0.1));
42  const mcl_3dl::State6DOF odom_current(odom_prev.pos_ + odom_prev.rot_ * relative_trans,
43  odom_prev.rot_ * relative_quat);
44 
45  const mcl_3dl::Vec3 state_trans(1.5, 1.2, 1.3);
46  const mcl_3dl::Quat state_quat(mcl_3dl::Vec3(-0.3, -0.2, -0.1));
47  mcl_3dl::State6DOF state(state_trans, state_quat);
48  state.noise_ll_ = 0.0;
49  state.noise_la_ = 0.0;
50  state.noise_al_ = 0.0;
51  state.noise_aa_ = 0.0;
52 
54  predictor.setOdoms(odom_prev, odom_current, 1.0);
55  predictor.predict(state);
56 
57  const auto expected_state_pos = state_trans + state_quat * relative_trans;
58  EXPECT_NEAR(state.pos_.x_, expected_state_pos.x_, EPS);
59  EXPECT_NEAR(state.pos_.y_, expected_state_pos.y_, EPS);
60  EXPECT_NEAR(state.pos_.z_, expected_state_pos.z_, EPS);
61 
62  const mcl_3dl::Quat expected_state_rot = state_quat * relative_quat;
63  EXPECT_NEAR(state.rot_.x_, expected_state_rot.x_, EPS);
64  EXPECT_NEAR(state.rot_.y_, expected_state_rot.y_, EPS);
65  EXPECT_NEAR(state.rot_.z_, expected_state_rot.z_, EPS);
66  EXPECT_NEAR(state.rot_.w_, expected_state_rot.w_, EPS);
67 
68  // odom_err_integ_lin_ and odom_err_integ_ang_ are zero vectors.
69  EXPECT_NEAR(state.odom_err_integ_lin_.x_, 0.0, EPS);
70  EXPECT_NEAR(state.odom_err_integ_lin_.y_, 0.0, EPS);
71  EXPECT_NEAR(state.odom_err_integ_lin_.z_, 0.0, EPS);
72  EXPECT_NEAR(state.odom_err_integ_ang_.x_, 0.0, EPS);
73  EXPECT_NEAR(state.odom_err_integ_ang_.y_, 0.0, EPS);
74  EXPECT_NEAR(state.odom_err_integ_ang_.z_, 0.0, EPS);
75 }
76 
77 TEST(TestMotionPredictionModelDifferentialDrive, predictWithoutRotation)
78 {
79  const mcl_3dl::Vec3 relative_trans(-0.5, 0.1, 0.05);
80  const float relative_trans_norm = relative_trans.norm();
81 
82  const mcl_3dl::State6DOF odom_prev(mcl_3dl::Vec3(5.0, 4.0, 3.0), mcl_3dl::Vec3(0.5, 0.4, 0.3));
83  const mcl_3dl::State6DOF odom_current(odom_prev.pos_ + odom_prev.rot_ * relative_trans, odom_prev.rot_);
84 
85  const mcl_3dl::Vec3 state_trans(4.0, 3.0, 2.0);
86  const float state_yaw_angle = 0.3;
87  const mcl_3dl::Quat state_quat(mcl_3dl::Vec3(0.0, 0.0, state_yaw_angle));
88  mcl_3dl::State6DOF state(state_trans, state_quat);
89  state.noise_ll_ = 0.1;
90  state.noise_al_ = 0.2;
91  state.noise_la_ = 0.3;
92  state.noise_aa_ = 0.4;
93 
94  const float err_integ_multiply = 0.9;
96  predictor.setOdoms(odom_prev, odom_current, 1.0);
97  predictor.predict(state);
98 
99  const auto expected_state_pos = state_trans + state_quat * relative_trans * (1.0 + state.noise_ll_);
100  EXPECT_NEAR(state.pos_.x_, expected_state_pos.x_, EPS);
101  EXPECT_NEAR(state.pos_.y_, expected_state_pos.y_, EPS);
102  EXPECT_NEAR(state.pos_.z_, expected_state_pos.z_, EPS);
103 
104  const float yaw_diff = state.noise_la_ * relative_trans_norm;
105  const mcl_3dl::Quat expected_state_quat(mcl_3dl::Vec3(0.0, 0.0, state_yaw_angle + yaw_diff));
106  EXPECT_NEAR(state.rot_.x_, expected_state_quat.x_, EPS);
107  EXPECT_NEAR(state.rot_.y_, expected_state_quat.y_, EPS);
108  EXPECT_NEAR(state.rot_.z_, expected_state_quat.z_, EPS);
109  EXPECT_NEAR(state.rot_.w_, expected_state_quat.w_, EPS);
110 
111  const auto expected_odom_err_integ_lin = (relative_trans * state.noise_ll_) * err_integ_multiply;
112  EXPECT_NEAR(state.odom_err_integ_lin_.x_, expected_odom_err_integ_lin.x_, EPS);
113  EXPECT_NEAR(state.odom_err_integ_lin_.y_, expected_odom_err_integ_lin.y_, EPS);
114  EXPECT_NEAR(state.odom_err_integ_lin_.z_, expected_odom_err_integ_lin.z_, EPS);
115 
116  const float expected_odom_err_integ_acc_z = yaw_diff * err_integ_multiply;
117  EXPECT_NEAR(state.odom_err_integ_ang_.x_, 0.0, EPS);
118  EXPECT_NEAR(state.odom_err_integ_ang_.y_, 0.0, EPS);
119  EXPECT_NEAR(state.odom_err_integ_ang_.z_, expected_odom_err_integ_acc_z, EPS);
120 }
121 
122 TEST(TestMotionPredictionModelDifferentialDrive, predictWithoutTranslationAndRollPitch)
123 {
124  const float relative_yaw_angle = 0.2;
125  const mcl_3dl::Quat relative_quat(mcl_3dl::Vec3(0.0, 0.0, relative_yaw_angle));
126 
127  const mcl_3dl::State6DOF odom_prev(mcl_3dl::Vec3(-5.0, -2.0, -3.0), mcl_3dl::Vec3(0.5, 1.0, 2.0));
128  const mcl_3dl::State6DOF odom_current(odom_prev.pos_, odom_prev.rot_ * relative_quat);
129 
130  const mcl_3dl::Vec3 state_trans(-5.5, -3.0, -2.0);
131  const float state_yaw_angle = 0.3;
132  const mcl_3dl::Quat state_quat(mcl_3dl::Vec3(0.0, 0.0, state_yaw_angle));
133  mcl_3dl::State6DOF state(state_trans, state_quat);
134  state.noise_ll_ = 0.1;
135  state.noise_al_ = 0.2;
136  state.noise_la_ = 0.3;
137  state.noise_aa_ = 0.4;
138 
139  const float err_integ_multiply = 1.0 - (1.0 / 10.0); // single step of the exponential smoother
141  predictor.setOdoms(odom_prev, odom_current, 1.0);
142  predictor.predict(state);
143 
144  const auto expected_state_pos =
145  state_trans + state_quat * mcl_3dl::Vec3(state.noise_al_ * relative_yaw_angle, 0.0, 0.0);
146  EXPECT_NEAR(state.pos_.x_, expected_state_pos.x_, EPS);
147  EXPECT_NEAR(state.pos_.y_, expected_state_pos.y_, EPS);
148  EXPECT_NEAR(state.pos_.z_, expected_state_pos.z_, EPS);
149 
150  const float yaw_diff = state.noise_aa_ * relative_yaw_angle;
151  const mcl_3dl::Quat expected_state_quat(mcl_3dl::Vec3(0.0, 0.0, relative_yaw_angle + state_yaw_angle + yaw_diff));
152  EXPECT_NEAR(state.rot_.x_, expected_state_quat.x_, EPS);
153  EXPECT_NEAR(state.rot_.y_, expected_state_quat.y_, EPS);
154  EXPECT_NEAR(state.rot_.z_, expected_state_quat.z_, EPS);
155  EXPECT_NEAR(state.rot_.w_, expected_state_quat.w_, EPS);
156 
157  const auto expected_odom_err_integ_lin =
158  mcl_3dl::Vec3(state.noise_al_ * relative_yaw_angle, 0.0, 0.0) * err_integ_multiply;
159  EXPECT_NEAR(state.odom_err_integ_lin_.x_, expected_odom_err_integ_lin.x_, EPS);
160  EXPECT_NEAR(state.odom_err_integ_lin_.y_, expected_odom_err_integ_lin.y_, EPS);
161  EXPECT_NEAR(state.odom_err_integ_lin_.z_, expected_odom_err_integ_lin.z_, EPS);
162 
163  const float expected_odom_err_integ_acc_z = yaw_diff * err_integ_multiply;
164  EXPECT_NEAR(state.odom_err_integ_ang_.x_, 0.0, EPS);
165  EXPECT_NEAR(state.odom_err_integ_ang_.y_, 0.0, EPS);
166  EXPECT_NEAR(state.odom_err_integ_ang_.z_, expected_odom_err_integ_acc_z, EPS);
167 }
168 
169 TEST(TestMotionPredictionModelDifferentialDrive, predictWithoutRollPitch)
170 {
171  const mcl_3dl::Vec3 relative_trans(1.0, 0.2, 0.3);
172  const float relative_trans_norm = relative_trans.norm();
173  const float relative_yaw_angle = 0.2;
174  const mcl_3dl::Quat relative_quat(mcl_3dl::Vec3(0.0, 0.0, relative_yaw_angle));
175 
176  const mcl_3dl::State6DOF odom_prev(mcl_3dl::Vec3(10.0, -5.0, 0.5), mcl_3dl::Vec3(-2.0, -1.0, -0.5));
177  const mcl_3dl::State6DOF odom_current(odom_prev.pos_ + odom_prev.rot_ * relative_trans,
178  odom_prev.rot_ * relative_quat);
179 
180  const mcl_3dl::Vec3 state_trans(11.0, -5.0, 1.0);
181  const float state_yaw_angle = 0.3;
182  const mcl_3dl::Quat state_quat(mcl_3dl::Vec3(0.0, 0.0, state_yaw_angle));
183  mcl_3dl::State6DOF state(state_trans, state_quat);
184  state.noise_ll_ = 0.1;
185  state.noise_al_ = 0.2;
186  state.noise_la_ = 0.3;
187  state.noise_aa_ = 0.4;
188 
189  const float err_integ_multiply = 1.0 - (1.0 / 10.0); // single step of the exponential smoother
191  predictor.setOdoms(odom_prev, odom_current, 1.0);
192  predictor.predict(state);
193 
194  const auto expected_state_pos = state_trans +
195  state_quat * (relative_trans * (1.0 + state.noise_ll_) +
196  mcl_3dl::Vec3(state.noise_al_ * relative_yaw_angle, 0.0, 0.0));
197  EXPECT_NEAR(state.pos_.x_, expected_state_pos.x_, EPS);
198  EXPECT_NEAR(state.pos_.y_, expected_state_pos.y_, EPS);
199  EXPECT_NEAR(state.pos_.z_, expected_state_pos.z_, EPS);
200 
201  const double yaw_diff = state.noise_la_ * relative_trans_norm + state.noise_aa_ * relative_yaw_angle;
202  const mcl_3dl::Quat expected_state_quat(mcl_3dl::Vec3(0.0, 0.0, relative_yaw_angle + state_yaw_angle + yaw_diff));
203  EXPECT_NEAR(state.rot_.x_, expected_state_quat.x_, EPS);
204  EXPECT_NEAR(state.rot_.y_, expected_state_quat.y_, EPS);
205  EXPECT_NEAR(state.rot_.z_, expected_state_quat.z_, EPS);
206  EXPECT_NEAR(state.rot_.w_, expected_state_quat.w_, EPS);
207 
208  const auto expected_odom_err_integ_lin =
209  (relative_trans * state.noise_ll_ + mcl_3dl::Vec3(state.noise_al_ * relative_yaw_angle, 0.0, 0.0)) *
210  err_integ_multiply;
211  EXPECT_NEAR(state.odom_err_integ_lin_.x_, expected_odom_err_integ_lin.x_, EPS);
212  EXPECT_NEAR(state.odom_err_integ_lin_.y_, expected_odom_err_integ_lin.y_, EPS);
213  EXPECT_NEAR(state.odom_err_integ_lin_.z_, expected_odom_err_integ_lin.z_, EPS);
214 
215  const float expected_odom_err_integ_acc_z = yaw_diff * err_integ_multiply;
216  EXPECT_NEAR(state.odom_err_integ_ang_.x_, 0.0, EPS);
217  EXPECT_NEAR(state.odom_err_integ_ang_.y_, 0.0, EPS);
218  EXPECT_NEAR(state.odom_err_integ_ang_.z_, expected_odom_err_integ_acc_z, EPS);
219 }
220 
221 int main(int argc, char** argv)
222 {
223  testing::InitGoogleTest(&argc, argv);
224 
225  return RUN_ALL_TESTS();
226 }
float z_
Definition: vec3.h:40
int main(int argc, char **argv)
TEST(TestMotionPredictionModelDifferentialDrive, predictWithoutNoise)
float y_
Definition: quat.h:43
mcl_3dl::Vec3 pos_
Definition: state_6dof.h:47
mcl_3dl::Vec3 odom_err_integ_ang_
Definition: state_6dof.h:55
float norm() const
Definition: vec3.h:149
mcl_3dl::Vec3 odom_err_integ_lin_
Definition: state_6dof.h:54
float w_
Definition: quat.h:45
float y_
Definition: vec3.h:39
void setOdoms(const State6DOF &odom_prev, const State6DOF &odom_current, const float time_diff) final
mcl_3dl::Quat rot_
Definition: state_6dof.h:48
float x_
Definition: vec3.h:38
float z_
Definition: quat.h:44
float x_
Definition: quat.h:42


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Mon Jul 8 2019 03:32:36