From 23c91a032244c032ad05930fbc5f283b73b340fe Mon Sep 17 00:00:00 2001
From: Rescue Group <rescue@rescuelap3.(none)>
Date: Thu, 25 Jul 2013 16:55:35 +1000
Subject: [PATCH] MM: Added motion copy option.

---
 .../include/crosbot_fastslam/heightmap.hpp    |  4 +++-
 crosbot_fastslam/src/fastslam.cpp             | 23 +++++++++++++++----
 2 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/crosbot_fastslam/include/crosbot_fastslam/heightmap.hpp b/crosbot_fastslam/include/crosbot_fastslam/heightmap.hpp
index b324ad2..145128c 100644
--- a/crosbot_fastslam/include/crosbot_fastslam/heightmap.hpp
+++ b/crosbot_fastslam/include/crosbot_fastslam/heightmap.hpp
@@ -53,13 +53,14 @@ public:
 
 	double minDistanceBetweenSnaps;
 
-	Pose searchPose;
+	crosbot::Pose searchPose;
 	double searchDistance, searchFOV;
 	double maxSensorRange;
 
 	unsigned int mapRows, mapColumns, patchRows, patchColumns;
 
 	double maxHeight, minHeight;
+	bool addMotionCopy;
 
 	FastSLAMParameters() {
 		numberOfParticles = DEFAULT_NUMPARTICLES;
@@ -81,6 +82,7 @@ public:
 
 		minHeight = 0; maxHeight = INFINITY;
 		maxSensorRange = INFINITY;
+		addMotionCopy = false;
 	}
 };
 
diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp
index 462dc0d..840fd63 100644
--- a/crosbot_fastslam/src/fastslam.cpp
+++ b/crosbot_fastslam/src/fastslam.cpp
@@ -77,6 +77,8 @@ void FastSLAMMap::configure(ConfigElementPtr config) {
 	if (searchConfig->hasParam("pose")) {
 		parameters.searchPose = searchConfig->getParamAsPose("pose", parameters.searchPose);
 	}
+
+	parameters.addMotionCopy = searchConfig->getParamAsBool("add_motion_copy", parameters.addMotionCopy);
 }
 
 void FastSLAMMap::start() {
@@ -509,12 +511,18 @@ void FastSLAMMap::resample() {
 
 	newParticles.resize(parameters.numberOfParticles);
 	newPoses.resize(newParticles.size());
-	for (size_t i = 0; i < newParticles.size(); i++) {
+	size_t i = 0;
+	if (parameters.addMotionCopy) {
+		newParticles[i] = new Particle(motion);
+		++i;
+	}
+
+	for (; i < newParticles.size(); i++) {
 		double rand = ProbabilityTable.randxy(0, weightSum);
 		ParticlePtr p = particles[0];
 		double wCurrent = p->weight;
 		size_t j = 0;
-		for (j++; j < particles.size() && wCurrent <= rand; j++) {
+		for (++j; j < particles.size() && wCurrent <= rand; j++) {
 			p = particles[j];
 			wCurrent += p->weight;
 		}
@@ -572,12 +580,19 @@ void FastSLAMMap::resampleAndUpdate(MapCloudPtr cloud) {
 	updates.push_back(motionJob);
 	workers.addJob(motionJob);
 
-	for (size_t i = 0; i < parameters.numberOfParticles; i++) {
+	size_t i = 0;
+	if (parameters.addMotionCopy) {
+		ParticleCopyJob *copy = new ParticleCopyJob(motion, newParticles, newPoses, newParticleMutex);
+		copies.push_back(copy);
+		++i;
+	}
+
+	for (; i < parameters.numberOfParticles; ++i) {
 		double rand = ProbabilityTable.randxy(0, weightSum);
 		ParticlePtr p = particles[0];
 		double wCurrent = p->weight;
 		size_t j = 0;
-		for (j++; j < particles.size() && wCurrent <= rand; j++) {
+		for (++j; j < particles.size() && wCurrent <= rand; ++j) {
 			p = particles[j];
 			wCurrent += p->weight;
 		}
-- 
GitLab