robotikka/Particle.cpp at master · Schniz/robotikka · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#include "Particle.h"

Particle::Particle() {
	this->x = 0;
	this->y = 0;
	this->yaw = 0;
	this->belief = 0;
	this->maxDistance = 0;
}

Particle::Particle(float x, float y, float yaw, float belief) {
	this->x = x;
	this->y = y;
	this->yaw = yaw;
	this->belief = belief;
	this->maxDistance = 0;
}

void Particle::SetMap(AnotherMap* map) {
	this->map = map;
}

void Particle::Update(float deltaX, float deltaY, float deltaYaw,
		float laserArray[], Cell* nextWaypoint) {
	this->x += deltaX;
	this->y += deltaY;
	this->yaw += deltaYaw;

	float predictionBelif = ProbMovement(deltaX, deltaY, deltaYaw)
			* this->belief;

	// TODO: check ?!
	float probabilityByScan = ProbByScan(laserArray);

	this->belief = probabilityByScan * predictionBelif * 2;

	if (this->belief > 1)
		this->belief = 1;
}

float Particle::ProbMovement(float deltaX, float deltaY, float deltaYaw) {
	float distance = sqrt(pow(deltaX, 2) + pow(deltaY, 2)); // cm?

	float prob = 0;

	prob = 1 - ((distance / this->maxDistance) * (deltaYaw / 360));

	if (prob > 1)
		prob = 1;

	return prob;
}

float Particle::ProbByScan(float laserArray[]) {
	int matchCount = 0;
	int countCheck = 0;
	int mapx = 0;
	int mapy = 0;

	int scanCount = laserProxy->GetCount();

	for (int i = 0; i < scanCount; i += 5) {
		countCheck++;
		int angle = laserProxy->GetBearing(i);
		double distanceFromLaserInPx = Utils::MathUtil::cmToPx(
				(double) laserArray[i] * 100.0);
		mapx = round(
				cos(DTOR(this->yaw) + angle) * distanceFromLaserInPx
						+ (double) this->x);
		mapy = round(
				sin(DTOR(this->yaw) + angle) * distanceFromLaserInPx
						+ (double) this->y);

		Cell* cell = this->map->getResizedCell(mapx, mapy);

		if (cell != NULL) {
			if (laserArray[i] < MAX_LEASER_DISTANCE
					&& cell->Cell_Cost == CellType::WALL) {
				matchCount++;
			}

			else if (cell->Cell_Cost != CellType::WALL) {
				matchCount++;
			}
		}

	}

	return matchCount / countCheck;

}

float Particle::Randomize(float min, float max) {
	float num = (float) rand() / RAND_MAX;
	return min + num * (max - min);
}

void Particle::SetMaxDistance(double maxDistance) {
	this->maxDistance = maxDistance;
}

Particle* Particle::CreateChild(float dExpansionRadius, float dYawRange) {
	float newX = this->x + Randomize(-dExpansionRadius, dExpansionRadius);
	float newY = this->y + Randomize(-dExpansionRadius, dExpansionRadius);
	float newYaw = this->yaw + Randomize(-dYawRange, dYawRange);
	Particle* p = new Particle(newX, newY, newYaw, 1);
	p->SetMap(this->map);
	p->SetMaxDistance(this->maxDistance);
	p->SetLaserProxy(this->laserProxy);

	return p;
}

Particle::~Particle() {
}