ParticleFilter-ROS2/particle.py at main · aalghooneh/ParticleFilter-ROS2 · 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

from mapUtilities import *
from utilities import *
from numpy import cos, sin
import numpy as np

class particle:

    def __init__(self, pose, weight):
        self.pose=pose
        self.weight=weight


    def motion_model(self,dth, v, w, dt):
        self.pose[0]=v * np.cos(self.pose[2]) * dt + self.pose[0]
        self.pose[1]=v * np.sin(self.pose[2]) * dt + self.pose[1]
        self.pose[2]=dth + self.pose[2]


    def calculateParticleWeight(self, scanOutput: LaserScan, mapManipulatorInstance: mapManipulator):

        T = self.__poseToTranslationMatrix()

        _, scanCartesianHomo = convertScanToCartesian(scanOutput)
        scanInMap = np.dot(T, scanCartesianHomo.T).T

        likelihoodField = mapManipulatorInstance.getLikelihoodField()

        origin = mapManipulatorInstance.getOrigin()
        res    = mapManipulatorInstance.getResolution()
        w      = mapManipulatorInstance.width
        h      = mapManipulatorInstance.height
        cellOrigin    = position_2_cell(np.array([0,0]), origin, res, h)
        cellPositions = position_2_cell(scanInMap[:,0:2], origin, res, h)
        cellParticle  = position_2_cell(self.getPose()[0:2], origin, res, h)

        lm_x, lm_y = likelihoodField.shape

        cellPositions = cellPositions[
            np.logical_and(cellPositions[:,0] < lm_y , -cellPositions[:,1] < lm_x), :
        ]

        log_weights = np.log(likelihoodField[-cellPositions[:, 1], cellPositions[:, 0]])
        log_weight = np.sum(log_weights)
        weight = np.exp(log_weight)
        weight+=1.e-300

        self.setWeight(weight)


    def setWeight(self, weight):
        self.weight = weight

    def getWeight(self):
        return self.weight

    def setPose(self, pose):
        self.pose = pose

    def getPose(self):
        return self.pose[0], self.pose[1], self.pose[2]


    def __poseToTranslationMatrix(self):
        x, y, th = self.getPose()


        translation = np.array([[cos(th), -sin(th),x],
                                [sin(th), cos(th),y],
                                [0,0,1]])

        return translation