From 757a852f2af8ac59c40b9d780b85dfa46be2e85c Mon Sep 17 00:00:00 2001
From: Guilherme Lawless <guilawless@live.com>
Date: Sun, 15 Jan 2017 18:22:38 +0000
Subject: [PATCH] Add some randomization after odometry model if robot does not
 see any landmark

---
 .../pfuclt_omni_dataset/pfuclt_particles.h    |  1 +
 src/pfuclt_particles.cpp                      | 33 +++++++++++++++----
 2 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/include/pfuclt_omni_dataset/pfuclt_particles.h b/include/pfuclt_omni_dataset/pfuclt_particles.h
index 80a09a4..103f6fb 100644
--- a/include/pfuclt_omni_dataset/pfuclt_particles.h
+++ b/include/pfuclt_omni_dataset/pfuclt_particles.h
@@ -314,6 +314,7 @@ class ParticleFilter
   ros::Publisher velPublisher_;
   ros::Time latestObservationTime_, savedLatestObservationTime_;
   bool converged_;
+  std::vector<bool> robotRandom;
 
   /**
    * @brief copyParticle - copies a whole particle from one particle set to
diff --git a/src/pfuclt_particles.cpp b/src/pfuclt_particles.cpp
index 354517a..a44485c 100644
--- a/src/pfuclt_particles.cpp
+++ b/src/pfuclt_particles.cpp
@@ -48,7 +48,7 @@ ParticleFilter::ParticleFilter(struct PFinitData& data)
       targetIterationTime_(), odometryTime_(), iterationEvalTime_(), mutex_(),
       dynamicServer_(), O_TARGET(data.nRobots * data.statesPerRobot),
       O_WEIGHT(nSubParticleSets_ - 1), numberIterations(0),
-      durationSum(ros::WallDuration(0))
+      durationSum(ros::WallDuration(0)), robotRandom(data.nRobots, false)
 {
   ROS_INFO("Created particle filter with dimensions %d, %d",
            (int)particles_.size(), (int)particles_[0].size());
@@ -283,10 +283,16 @@ void ParticleFilter::fuseRobots()
       ROS_WARN("In this iteration, OMNI%d didn't see any landmarks, so the "
                "fusing will be skipped for it",
                r + 1);
+
+      robotRandom[r] = true;
+
       continue;
     }
     else
+    {
+      robotRandom[r] = false;
       weightComponents_[r] = probabilities[r];
+    }
 
     // Index offset for this robot in the particles vector
     uint o_robot = r * nStatesPerRobot_;
@@ -434,14 +440,14 @@ void ParticleFilter::fuseTarget()
         mStar = p;
       }
     }
+  }
 
-    // Particle m* has been found, let's swap the subparticles
-    for (uint i = 0; i < STATES_PER_TARGET; ++i)
-      std::swap(particles_[O_TARGET + i][m], particles_[O_TARGET + i][mStar]);
+  // Particle m* has been found, let's swap the subparticles
+  for (uint i = 0; i < STATES_PER_TARGET; ++i)
+    std::swap(particles_[O_TARGET + i][m], particles_[O_TARGET + i][mStar]);
 
-    // Update the weight of this particle
-    particles_[O_WEIGHT][m] *= maxTargetSubParticleWeight;
-  }
+  // Update the weight of this particle
+  particles_[O_WEIGHT][m] *= maxTargetSubParticleWeight;
 
   // The target subparticles are now reordered according to their weight
   // contribution
@@ -823,6 +829,19 @@ void ParticleFilter::predict(const uint robotNumber, const Odometry odom,
         particles_[O_THETA + robot_offset][i] + deltaFinalRotEffective(seed_));
   }
 
+  if (robotRandom[robotNumber] || !converged_)
+  {
+    // Randomize a bit for this robot since it does not see landmarks and target
+    // isn't seen
+    boost::random::uniform_real_distribution<> randPar(-0.05, 0.05);
+
+    for (uint p = 0; p < nParticles_; ++p)
+    {
+      for (uint s = 0; s < nStatesPerRobot_; ++s)
+        particles_[robot_offset + s][p] += randPar(seed_);
+    }
+  }
+
   // If this is the main robot, perform one PF-UCLT iteration
   if (mainRobotID_ == robotNumber)
   {