VisualOdometryUI/Position.py at master · firiceguo/VisualOdometryUI · 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
116
117
import math
import Translation
import Rotation
import lk_track
from PyQt4 import QtCore
import numpy as np
import cv2
import matplotlib.pyplot as plt


class Position(QtCore.QThread):
    """docstring for Position"""

    _signalPosition = QtCore.pyqtSignal()

    def __init__(self, track=lk_track.TrackLK):
        super(Position, self).__init__(track)
        self.rotation = Rotation.Rotation(track)
        self.translation = Translation.Translation(self.rotation.GetRad(), track)

        self.rotationSum = 0
        self.pathLen = 0
        self.location = [0, 0]
        self.loc = []
        self.trackP = track
        self.heading = 0
        self.trans = 0.059

        self.flag = 1
        self.timer = QtCore.QTimer(QtCore.QThread())

    def run(self):
        perspectiveMatrix = self.GetPerspectiveMatrix()
        x = []
        y = []
        while self.flag:
            self.CalculatePosition(perspectiveMatrix)
            self._signalPosition.emit()
            if not self.trackP.fps:
                for i in xrange(len(self.loc)):
                    x.append(self.loc[i][0])
                    y.append(self.loc[i][1])
                plt.plot(x, y, 'o')
                plt.show()
                break

    def CalculatePosition(self, perspectiveMatrix):
        GoodFeatures = []
        GoodFeatures = self.DetermineGoodFeatures(self.trackP.GetTrackFeatures())
        self.rotation.run(GoodFeatures)

        prevHeading = self.heading
        self.heading += self.rotation.GetRad()
        currHeading = self.heading
        self.translation.run(self.rotation.GetRad(), perspectiveMatrix, GoodFeatures, prevHeading, currHeading)
        change = self.translation.GetCurrentLocationChange()
        self.location[0] += change[0]
        self.location[1] += change[1]
        self.pathLen += math.sqrt(change[0] * change[0] + change[1] * change[1])

    def SetZero(self):
        self.rotationSum = 0
        self.pathLen = 0
        self.location = [0, 0]
        self.heading = 0

    def GetDistance(self):
        ans = math.sqrt(self.location[0] * self.location[0] + self.location[1] * self.location[1]) * self.trans
        return(ans)

    def GetPathLen(self):
        ans = self.pathLen * self.trans
        return(ans)

    def GetHeading(self):
        return(math.degrees(self.heading))

    def GetLocation(self):
        ans = [self.location[0] * self.trans, - self.location[1] * self.trans]
        self.loc.append(ans)
        return(ans)

    def GetPerspectiveMatrix(self):
        img = cv2.imread("./data/perspectiveimg.jpg", 0)
        w = img.shape[1]
        h = img.shape[0]

        src = [[186, 361], [449, 361],
               [133, 466], [508, 466]]
        src = np.array(src, np.float32)
        dst = np.array([[w // 2 - 100, h - 200], [w // 2 + 100, h - 200],
                        [w // 2 - 100, h], [w // 2 + 100, h]], np.float32)
        ret = cv2.getPerspectiveTransform(src, dst)

        return(ret)

    def DetermineGoodFeatures(self, tracks):
        features = tracks
        GoodFeatures = []
        for i in xrange(len(features)):
            FeatureScore = 5
            for j in xrange(len(features[i]) - 1):
                if len(features[i]) < 5:
                    continue
                try:
                    PrevPoint = np.array(features[i][j])
                    CurrPoint = np.array(features[i][j + 1])
                except IndexError:
                    continue
                dis = np.linalg.norm(PrevPoint - CurrPoint)
                if dis > 15:
                    FeatureScore += 1
                else:
                    FeatureScore -= 1
            if FeatureScore < 6:
                GoodFeatures.append(features[i])
        return(GoodFeatures)