28 out.WriteAs<uint32_t>(estimationMethod);
30 out << hasVelocities << velocities;
49 in >> hasVelocities >> velocities;
66 estimationMethod = emOdometry;
67 rawOdometryIncrementReading = odometryIncrement;
68 motionModelConfiguration = options;
71 computeFromOdometry_model6DOF(odometryIncrement, options);
86 std::make_shared<CPose3DPDFParticles>();
148 double Ayaw1 = (odometryIncrement.
y() != 0 || odometryIncrement.
x() != 0)
149 ? atan2(odometryIncrement.
y(), odometryIncrement.
x())
152 double Atrans = odometryIncrement.
norm();
155 (odometryIncrement.
y() != 0 || odometryIncrement.
x() != 0 ||
156 odometryIncrement.z() != 0)
157 ? atan2(odometryIncrement.z(), odometryIncrement.
norm())
160 double Aroll = odometryIncrement.
roll();
161 double Apitch2 = odometryIncrement.
pitch();
162 double Ayaw2 = odometryIncrement.
yaw();
164 const auto stdxyz = motionModelConfiguration.mm6DOFModel.additional_std_XYZ;
173 rnd.drawGaussian1D_normalized();
174 double Apitch1_draw =
176 rnd.drawGaussian1D_normalized();
180 rnd.drawGaussian1D_normalized();
183 rnd.drawGaussian1D_normalized();
184 double Apitch2_draw = Apitch2 + (o.
mm6DOFModel.
a8 * Apitch2) *
185 rnd.drawGaussian1D_normalized();
188 rnd.drawGaussian1D_normalized();
191 ith_part.x = Atrans_draw * cos(Apitch1_draw) * cos(Ayaw1_draw) +
192 stdxyz * rnd.drawGaussian1D_normalized();
193 ith_part.y = Atrans_draw * cos(Apitch1_draw) * sin(Ayaw1_draw) +
194 stdxyz * rnd.drawGaussian1D_normalized();
195 ith_part.z = -Atrans_draw * sin(Apitch1_draw) +
196 stdxyz * rnd.drawGaussian1D_normalized();
199 Ayaw1_draw + Ayaw2_draw +
200 motionModelConfiguration.mm6DOFModel.additional_std_angle *
201 rnd.drawGaussian1D_normalized();
203 Apitch1_draw + Apitch2_draw +
204 motionModelConfiguration.mm6DOFModel.additional_std_angle *
205 rnd.drawGaussian1D_normalized();
208 motionModelConfiguration.mm6DOFModel.additional_std_angle *
209 rnd.drawGaussian1D_normalized();
212 poseChange.copyFrom(*poseChangeTemp);
219 o <<
"Robot Movement (as a gaussian pose change):\n";
220 o << poseChange <<
"\n";