20 #include <Eigen/Dense>
36 computeFromOdometry(rawOdometryIncrementReading, motionModelConfiguration);
43 out.WriteAs<uint32_t>(estimationMethod);
48 if (estimationMethod == emOdometry)
51 out << rawOdometryIncrementReading;
52 out.WriteAs<uint32_t>(motionModelConfiguration.modelSelection);
53 const auto& gm = motionModelConfiguration.gaussianModel;
54 out.WriteAs<
float>(gm.a1);
55 out.WriteAs<
float>(gm.a2);
56 out.WriteAs<
float>(gm.a3);
57 out.WriteAs<
float>(gm.a4);
58 out.WriteAs<
float>(gm.minStdXY);
59 out.WriteAs<
float>(gm.minStdPHI);
61 out << motionModelConfiguration.thrunModel.nParticlesCount
62 << motionModelConfiguration.thrunModel.alfa1_rot_rot
63 << motionModelConfiguration.thrunModel.alfa2_rot_trans
64 << motionModelConfiguration.thrunModel.alfa3_trans_trans
65 << motionModelConfiguration.thrunModel.alfa4_trans_rot
66 << motionModelConfiguration.thrunModel.additional_std_XY
67 << motionModelConfiguration.thrunModel.additional_std_phi;
77 if (hasVelocities)
out << velocityLocal;
79 out << hasEncodersInfo;
81 out << encoderLeftTicks << encoderRightTicks;
106 if (estimationMethod == emOdometry)
109 in >> rawOdometryIncrementReading;
112 motionModelConfiguration.modelSelection =
115 auto& gm = motionModelConfiguration.gaussianModel;
116 gm.a1 = in.
ReadAs<
float>();
117 gm.a2 = in.
ReadAs<
float>();
118 gm.a3 = in.
ReadAs<
float>();
119 gm.a4 = in.
ReadAs<
float>();
120 gm.minStdXY = in.
ReadAs<
float>();
121 gm.minStdPHI = in.
ReadAs<
float>();
124 motionModelConfiguration.thrunModel.nParticlesCount = i;
125 in >> motionModelConfiguration.thrunModel.alfa1_rot_rot >>
126 motionModelConfiguration.thrunModel.alfa2_rot_trans >>
127 motionModelConfiguration.thrunModel.alfa3_trans_trans >>
128 motionModelConfiguration.thrunModel.alfa4_trans_rot;
133 motionModelConfiguration.thrunModel.additional_std_XY >>
134 motionModelConfiguration.thrunModel.additional_std_phi;
138 motionModelConfiguration.thrunModel.additional_std_XY =
139 motionModelConfiguration.thrunModel.additional_std_phi =
145 rawOdometryIncrementReading, motionModelConfiguration);
158 if (hasVelocities) in >> velocityLocal;
162 float velocityLin, velocityAng;
163 in >> velocityLin >> velocityAng;
164 velocityLocal.vx = velocityLin;
165 velocityLocal.vy = .0f;
166 velocityLocal.omega = velocityAng;
168 in >> hasEncodersInfo;
169 if (version < 7 || hasEncodersInfo)
172 encoderLeftTicks = i;
174 encoderRightTicks = i;
178 encoderLeftTicks = 0;
179 encoderRightTicks = 0;
200 if (estimationMethod == emOdometry)
203 in >> rawOdometryIncrementReading;
206 motionModelConfiguration.modelSelection =
209 float dum1, dum2, dum3;
211 in >> dum1 >> dum2 >> dum3 >>
212 motionModelConfiguration.gaussianModel.minStdXY >>
213 motionModelConfiguration.gaussianModel.minStdPHI;
217 motionModelConfiguration.thrunModel.nParticlesCount = i;
218 in >> motionModelConfiguration.thrunModel.alfa1_rot_rot;
219 in >> motionModelConfiguration.thrunModel.alfa2_rot_trans >>
220 motionModelConfiguration.thrunModel.alfa3_trans_trans >>
221 motionModelConfiguration.thrunModel.alfa4_trans_rot;
225 rawOdometryIncrementReading, motionModelConfiguration);
237 float velocityLin, velocityAng;
238 in >> velocityLin >> velocityAng;
239 velocityLocal.vx = velocityLin;
240 velocityLocal.vy = .0f;
241 velocityLocal.omega = velocityAng;
243 in >> hasEncodersInfo;
246 encoderLeftTicks = i;
248 encoderRightTicks = i;
263 if (estimationMethod == emOdometry)
266 in >> rawOdometryIncrementReading;
278 rawOdometryIncrementReading, motionModelConfiguration);
290 float velocityLin, velocityAng;
291 in >> velocityLin >> velocityAng;
292 velocityLocal.vx = velocityLin;
293 velocityLocal.vy = .0f;
294 velocityLocal.omega = velocityAng;
296 in >> hasEncodersInfo;
298 encoderLeftTicks = i;
300 encoderRightTicks = i;
318 if (estimationMethod == emOdometry)
319 poseChange->getMean(rawOdometryIncrementReading);
321 rawOdometryIncrementReading =
CPose2D(0, 0, 0);
327 float velocityLin, velocityAng;
328 in >> velocityLin >> velocityAng;
329 velocityLocal.vx = velocityLin;
330 velocityLocal.vy = .0f;
331 velocityLocal.omega = velocityAng;
333 in >> hasEncodersInfo;
335 encoderLeftTicks = i;
337 encoderRightTicks = i;
342 hasVelocities = hasEncodersInfo =
false;
343 encoderLeftTicks = encoderRightTicks = 0;
357 double K_left,
double K_right,
double D)
362 0.5 * (K_right * encoderRightTicks + K_left * encoderLeftTicks);
364 (K_right * encoderRightTicks - K_left * encoderLeftTicks) / D;
369 const double R = As / Aphi;
371 y =
R * (1 - cos(Aphi));
380 computeFromOdometry(
CPose2D(x, y, Aphi), motionModelConfiguration);
391 estimationMethod = emOdometry;
392 rawOdometryIncrementReading = odometryIncrement;
393 motionModelConfiguration = options;
396 computeFromOdometry_modelGaussian(odometryIncrement, options);
398 computeFromOdometry_modelThrun(odometryIncrement, options);
409 poseChange = std::make_shared<CPosePDFGaussian>();
417 double Al = odometryIncrement.
norm();
433 double cos_Aphi_2 = cos(odometryIncrement.
phi() / 2);
434 double sin_Aphi_2 = sin(odometryIncrement.
phi() / 2);
436 H(0, 0) = H(1, 1) = cos_Aphi_2;
437 H(0, 1) = -(H(1, 0) = sin_Aphi_2);
442 J(0, 2) = -sin_Aphi_2 * ODO_INCR(0, 0) - cos_Aphi_2 * ODO_INCR(1, 0);
443 J(1, 2) = cos_Aphi_2 * ODO_INCR(0, 0) - sin_Aphi_2 * ODO_INCR(1, 0);
446 aux->mean = odometryIncrement;
462 poseChange = CPosePDFParticles::Create();
477 double Arot1 = (odometryIncrement.
y() != 0 || odometryIncrement.
x() != 0)
478 ? atan2(odometryIncrement.
y(), odometryIncrement.
x())
480 double Atrans = odometryIncrement.
norm();
501 aux->m_particles[i].d.x =
502 Atrans_draw * cos(Arot1_draw) +
503 motionModelConfiguration.thrunModel.additional_std_XY *
505 aux->m_particles[i].d.y =
506 Atrans_draw * sin(Arot1_draw) +
507 motionModelConfiguration.thrunModel.additional_std_XY *
509 aux->m_particles[i].d.phi =
510 Arot1_draw + Arot2_draw +
511 motionModelConfiguration.thrunModel.additional_std_phi *
513 aux->m_particles[i].d.normalizePhi();
524 if (estimationMethod == emOdometry)
526 if (motionModelConfiguration.modelSelection == mmGaussian)
527 drawSingleSample_modelGaussian(outSample);
529 drawSingleSample_modelThrun(outSample);
534 poseChange->drawSingleSample(outSample);
546 poseChange->drawSingleSample(outSample);
563 double Arot1 = (rawOdometryIncrementReading.y() != 0 ||
564 rawOdometryIncrementReading.x() != 0)
566 rawOdometryIncrementReading.y(),
567 rawOdometryIncrementReading.x())
569 double Atrans = rawOdometryIncrementReading.norm();
570 double Arot2 =
math::wrapToPi(rawOdometryIncrementReading.phi() - Arot1);
574 (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot1) +
575 motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
579 (motionModelConfiguration.thrunModel.alfa3_trans_trans * Atrans +
580 motionModelConfiguration.thrunModel.alfa4_trans_rot *
581 (fabs(Arot1) + fabs(Arot2))) *
585 (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot2) +
586 motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
591 Atrans_draw * cos(Arot1_draw) +
592 motionModelConfiguration.thrunModel.additional_std_XY *
595 Atrans_draw * sin(Arot1_draw) +
596 motionModelConfiguration.thrunModel.additional_std_XY *
599 Arot1_draw + Arot2_draw +
600 motionModelConfiguration.thrunModel.additional_std_phi *
612 if (estimationMethod == emOdometry)
614 if (motionModelConfiguration.modelSelection == mmGaussian)
615 prepareFastDrawSingleSample_modelGaussian();
617 prepareFastDrawSingleSample_modelThrun();
628 if (estimationMethod == emOdometry)
630 if (motionModelConfiguration.modelSelection == mmGaussian)
631 fastDrawSingleSample_modelGaussian(outSample);
633 fastDrawSingleSample_modelThrun(outSample);
638 poseChange->drawSingleSample(outSample);
655 m_fastDrawGauss_M = gPdf->mean;
662 std::vector<double> eigvals;
669 m_fastDrawGauss_Z = m_fastDrawGauss_Z * D;
685 rndVector.
assign(3, 0.0f);
686 for (
size_t i = 0; i < 3; i++)
689 for (
size_t d = 0; d < 3; d++)
690 rndVector[d] += (m_fastDrawGauss_Z(d, i) * rnd);
694 m_fastDrawGauss_M.x() + rndVector[0],
695 m_fastDrawGauss_M.y() + rndVector[1],
696 m_fastDrawGauss_M.phi() + rndVector[2]);
705 drawSingleSample_modelThrun(outSample);
714 poseChange->getCovarianceAndMean(mat, Ap);
716 o <<
"Robot Movement (as a gaussian pose change):\n";
717 o <<
" Mean = " << Ap <<
"\n";
719 o <<
format(
" Covariance: DET=%e\n", mat.
det());
721 o <<
format(
" %e %e %e\n", mat(0, 0), mat(0, 1), mat(0, 2));
722 o <<
format(
" %e %e %e\n", mat(1, 0), mat(1, 1), mat(1, 2));
723 o <<
format(
" %e %e %e\n", mat(2, 0), mat(2, 1), mat(2, 2));
727 o <<
"Actual odometry increment reading: " << rawOdometryIncrementReading
731 "Actual PDF class is: '%s'\n",
732 poseChange->GetRuntimeClass()->className);
737 std::dynamic_pointer_cast<CPosePDFParticles>(poseChange.get_ptr());
739 " (Particle count = %u)\n", (
unsigned)aux->m_particles.size());
743 o <<
"Estimation method: "
751 "Encoder info: deltaL=%i deltaR=%i\n", encoderLeftTicks,
754 o <<
"Encoder info: Not available!\n";
757 o <<
"Velocity info: v=" << velocityLocal.asString() <<
"\n";
759 o <<
"Velocity info: Not available!\n";