Skip to content

Commit 348bf9c

Browse files
KilianBerlomviethlarshg
authored
Fix bug for tracking object rotation (#5538)
* Fix bug for tracking object rotation The current object tracking particle filter fails to track full circle Hence, for each iteration we have to: 1. Convert angles to Cartesian coords and average x and y separately 2. Convert back to the angles using the arctangent Now PCL is able to track objects in full 360 rotation * Fix bug for tracking object rotation The current object tracking particle filter fails to track full circle Hence, for each iteration we have to: 1. Convert angles to Cartesian coords and average x and y separately 2. Convert back to the angles using the arctangent Now PCL is able to track objects in full 360 rotation * Weighted average function with specializations for particle pose * Removed empty lines for code formatting * Added empty lines in the end for code formatting * Formatting complies with the patch file * Formatting complies with the 2nd patch file * Formatting complies with the 3rd patch file * Formatting complies with the 4th patch file * Formatting complies with the 5th patch file * Implement WeightedAverage specializations as struct member functions * Implement correct WeightedAverage struct member functions * Adjusted weighted average calculations according to particle definitions * Small cosine calculation optimisation * Minor code quality adjustments * Resolved type definition confusion * Update tracking/include/pcl/tracking/impl/particle_filter.hpp Earlier on the .points has been used but its bad design to expose the internal data structure, so we do not want to use .points directly. Co-authored-by: Lars Glud <larshg@gmail.com> --------- Co-authored-by: Markus Vieth <mvieth@techfak.uni-bielefeld.de> Co-authored-by: Lars Glud <larshg@gmail.com>
1 parent 788c53e commit 348bf9c

File tree

3 files changed

+110
-4
lines changed

3 files changed

+110
-4
lines changed

‎tracking/include/pcl/tracking/impl/particle_filter.hpp

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -365,13 +365,11 @@ template <typename PointInT, typename StateT>
365365
void
366366
ParticleFilterTracker<PointInT, StateT>::update()
367367
{
368-
369368
StateT orig_representative = representative_state_;
370369
representative_state_.zero();
371370
representative_state_.weight = 0.0;
372-
for (const auto& p : *particles_) {
373-
representative_state_ = representative_state_ + p * p.weight;
374-
}
371+
representative_state_ =
372+
StateT::weightedAverage(particles_->begin(), particles_->end());
375373
representative_state_.weight = 1.0f / static_cast<float>(particles_->size());
376374
motion_ = representative_state_ - orig_representative;
377375
}

‎tracking/include/pcl/tracking/impl/tracking.hpp

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -129,6 +129,30 @@ struct EIGEN_ALIGN16 ParticleXYZRPY : public _ParticleXYZRPY {
129129
return {trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw};
130130
}
131131

132+
template <class InputIterator>
133+
static ParticleXYZRPY
134+
weightedAverage(InputIterator first, InputIterator last)
135+
{
136+
ParticleXYZRPY wa;
137+
float wa_roll_sin = 0.0, wa_roll_cos = 0.0, wa_pitch_sin = 0.0, wa_pitch_cos = 0.0,
138+
wa_yaw_sin = 0.0, wa_yaw_cos = 0.0;
139+
for (auto point = first; point != last; ++point) {
140+
wa.x += point->x * point->weight;
141+
wa.y += point->y * point->weight;
142+
wa.z += point->z * point->weight;
143+
wa_pitch_cos = std::cos(point->pitch);
144+
wa_roll_sin += wa_pitch_cos * std::sin(point->roll) * point->weight;
145+
wa_roll_cos += wa_pitch_cos * std::cos(point->roll) * point->weight;
146+
wa_pitch_sin += std::sin(point->pitch) * point->weight;
147+
wa_yaw_sin += wa_pitch_cos * std::sin(point->yaw) * point->weight;
148+
wa_yaw_cos += wa_pitch_cos * std::cos(point->yaw) * point->weight;
149+
}
150+
wa.roll = std::atan2(wa_roll_sin, wa_roll_cos);
151+
wa.pitch = std::asin(wa_pitch_sin);
152+
wa.yaw = std::atan2(wa_yaw_sin, wa_yaw_cos);
153+
return wa;
154+
}
155+
132156
// a[i]
133157
inline float
134158
operator[](unsigned int i)
@@ -295,6 +319,24 @@ struct EIGEN_ALIGN16 ParticleXYZR : public _ParticleXYZR {
295319
return {trans_x, trans_y, trans_z, 0, trans_pitch, 0};
296320
}
297321

322+
template <class InputIterator>
323+
static ParticleXYZR
324+
weightedAverage(InputIterator first, InputIterator last)
325+
{
326+
ParticleXYZR wa;
327+
float wa_pitch_sin = 0.0;
328+
for (auto point = first; point != last; ++point) {
329+
wa.x += point->x * point->weight;
330+
wa.y += point->y * point->weight;
331+
wa.z += point->z * point->weight;
332+
wa_pitch_sin += std::sin(point->pitch) * point->weight;
333+
}
334+
wa.roll = 0.0;
335+
wa.pitch = std::asin(wa_pitch_sin);
336+
wa.yaw = 0.0;
337+
return wa;
338+
}
339+
298340
// a[i]
299341
inline float
300342
operator[](unsigned int i)
@@ -461,6 +503,30 @@ struct EIGEN_ALIGN16 ParticleXYRPY : public _ParticleXYRPY {
461503
return {trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw};
462504
}
463505

506+
template <class InputIterator>
507+
static ParticleXYRPY
508+
weightedAverage(InputIterator first, InputIterator last)
509+
{
510+
ParticleXYRPY wa;
511+
float wa_roll_sin = 0.0, wa_roll_cos = 0.0, wa_pitch_sin = 0.0, wa_pitch_cos = 0.0,
512+
wa_yaw_sin = 0.0, wa_yaw_cos = 0.0;
513+
for (auto point = first; point != last; ++point) {
514+
wa.x += point->x * point->weight;
515+
wa.z += point->z * point->weight;
516+
wa_pitch_cos = std::cos(point->pitch);
517+
wa_roll_sin += wa_pitch_cos * std::sin(point->roll) * point->weight;
518+
wa_roll_cos += wa_pitch_cos * std::cos(point->roll) * point->weight;
519+
wa_pitch_sin += std::sin(point->pitch) * point->weight;
520+
wa_yaw_sin += wa_pitch_cos * std::sin(point->yaw) * point->weight;
521+
wa_yaw_cos += wa_pitch_cos * std::cos(point->yaw) * point->weight;
522+
}
523+
wa.y = 0;
524+
wa.roll = std::atan2(wa_roll_sin, wa_roll_cos);
525+
wa.pitch = std::asin(wa_pitch_sin);
526+
wa.yaw = std::atan2(wa_yaw_sin, wa_yaw_cos);
527+
return wa;
528+
}
529+
464530
// a[i]
465531
inline float
466532
operator[](unsigned int i)
@@ -627,6 +693,27 @@ struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP {
627693
return {trans_x, 0, trans_z, 0, trans_pitch, trans_yaw};
628694
}
629695

696+
template <class InputIterator>
697+
static ParticleXYRP
698+
weightedAverage(InputIterator first, InputIterator last)
699+
{
700+
ParticleXYRP wa;
701+
float wa_yaw_sin = 0.0, wa_yaw_cos = 0.0, wa_pitch_sin = 0.0, wa_pitch_cos = 0.0;
702+
for (auto point = first; point != last; ++point) {
703+
wa.x += point->x * point->weight;
704+
wa.z += point->z * point->weight;
705+
wa_pitch_cos = std::cos(point->pitch);
706+
wa_pitch_sin += std::sin(point->pitch) * point->weight;
707+
wa_yaw_sin += wa_pitch_cos * std::sin(point->yaw) * point->weight;
708+
wa_yaw_cos += wa_pitch_cos * std::cos(point->yaw) * point->weight;
709+
}
710+
wa.y = 0.0;
711+
wa.roll = 0.0;
712+
wa.pitch = std::asin(wa_pitch_sin);
713+
wa.yaw = std::atan2(wa_yaw_sin, wa_yaw_cos);
714+
return wa;
715+
}
716+
630717
// a[i]
631718
inline float
632719
operator[](unsigned int i)
@@ -793,6 +880,24 @@ struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR {
793880
return {trans_x, 0, trans_z, 0, trans_pitch, 0};
794881
}
795882

883+
template <class InputIterator>
884+
static ParticleXYR
885+
weightedAverage(InputIterator first, InputIterator last)
886+
{
887+
ParticleXYR wa;
888+
float wa_pitch_sin = 0.0;
889+
for (auto point = first; point != last; ++point) {
890+
wa.x += point->x * point->weight;
891+
wa.z += point->z * point->weight;
892+
wa_pitch_sin += std::sin(point->pitch) * point->weight;
893+
}
894+
wa.y = 0.0;
895+
wa.roll = 0.0;
896+
wa.pitch = std::asin(wa_pitch_sin);
897+
wa.yaw = 0.0;
898+
return wa;
899+
}
900+
796901
// a[i]
797902
inline float
798903
operator[](unsigned int i)

‎tracking/include/pcl/tracking/tracking.h

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,10 @@ namespace pcl {
4545
namespace tracking {
4646
/* state definition */
4747
struct ParticleXYZRPY;
48+
struct ParticleXYRPY;
49+
struct ParticleXYRP;
4850
struct ParticleXYR;
51+
struct ParticleXYZR;
4952

5053
/* \brief return the value of normal distribution */
5154
PCL_EXPORTS double

0 commit comments

Comments
 (0)