~estraw/visual-odometry

ref: 811ef389135491e9be7387b20737d41f69ea1b10 visual-odometry/src/visual_odometry.py -rw-r--r-- 8.3 KiB
811ef389Evan Straw Add pause feature to demo program 4 months ago
                                                                                
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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
#
# Library that implements visual odometry.
#
# Copyright 2021 Evan Straw
#
# Licensed under the Apache License, Version 2.0 (the "License"); you may not
# use this file except in compliance with the License.  You may obtain a copy
# of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
# License for the specific language governing permissions and limitations under
# the License.
#

import video_input as vi
import cv2
import numpy as np
import math

FLANN_INDEX_KDTREE = 1


class VisualOdometry:
    def __init__(self, P1, P2):
        # Initialize FLANN matcher for features.
        index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
        search_params = dict(checks=100)
        self.__matcher = cv2.FlannBasedMatcher(index_params, search_params)

        # Initialize SIFT feature detector/descriptor.
        self.__descriptor = cv2.SIFT_create()
        self.__detector = cv2.FastFeatureDetector_create()

        self.__prev_frame = None
        self.__frame = None

        self.__time_matches = None
        self.__P1 = P1
        self.__P2 = P2

    def start(self, first_frame: vi.StereoFrame):
        if first_frame is None:
            raise ValueError("first frame cannot be empty")
        self.__frame = first_frame

    # Detect and compute descriptors for features in each frame.
    def __detectAndCompute(self):
        lp = cv2.goodFeaturesToTrack(self.__prev_frame.left_gray,
                                     500, 0.01, 10)
        rp = cv2.goodFeaturesToTrack(self.__prev_frame.right_gray,
                                     500, 0.01, 10)
        lkp = cv2.KeyPoint_convert(lp)
        rkp = cv2.KeyPoint_convert(rp)
        lkp, ldesc = self.__descriptor.compute(self.__prev_frame.left, lkp)
        rkp, rdesc = self.__descriptor.compute(self.__prev_frame.right, rkp)
        return lkp, rkp, ldesc, rdesc

    def __filterMatches(self, knnMatches, thresh):
        # Take the two closest matches to each descriptor; if the ratio of
        # match distances is above a certain threshold (i.e. the two nearest
        # matches are very near each other) discard the match entirely.
        good_matches = []
        for i, (m, n) in enumerate(knnMatches):
            if m.distance / n.distance < thresh:
                good_matches.append(m)
        return good_matches

    def __getMatchPoints(self, lkp, rkp, matches):
        # For all good matches, add corresponding keypoint indices to
        # respective arrays for use as a filter later.
        lIndices = []
        rIndices = []
        for m in matches:
            lIndices.append(m.queryIdx)
            rIndices.append(m.trainIdx)

        lPoints = cv2.KeyPoint_convert(lkp, lIndices)
        rPoints = cv2.KeyPoint_convert(rkp, rIndices)
        return lPoints, rPoints

    def __trackPrevToCurrent(self, lPoints, rPoints):
        lp_cur, lstat, lerr = cv2.calcOpticalFlowPyrLK(
            self.__prev_frame.left_gray,
            self.__frame.left_gray,
            lPoints, None)
        rp_cur, rstat, rerr = cv2.calcOpticalFlowPyrLK(
            self.__prev_frame.right_gray,
            self.__frame.right_gray,
            rPoints, None)
        good_lpoints_prev = []
        good_rpoints_prev = []
        good_lpoints_cur = []
        good_rpoints_cur = []
        for i in range(0, min(len(lstat), len(rstat))):
            if lstat[i] == 1 and rstat[i] == 1:
                good_lpoints_prev.append(lPoints[i])
                good_rpoints_prev.append(rPoints[i])
                good_lpoints_cur.append(lp_cur[i])
                good_rpoints_cur.append(rp_cur[i])
        return ((good_lpoints_prev, good_lpoints_cur),
                (good_rpoints_prev, good_rpoints_cur))

    def __drawTimeMatches(self, frame, prev_frame, matches, alpha,
                          prev_color, cur_color, line_color):
        out = cv2.addWeighted(frame, alpha, prev_frame, 1, 0)
        for (prev, cur) in zip(matches[0], matches[1]):
            prev = tuple(prev)
            cur = tuple(cur)
            cv2.line(out, prev, cur, line_color)
            cv2.drawMarker(out, prev, prev_color, cv2.MARKER_CROSS, 10)
            cv2.drawMarker(out, cur, cur_color, cv2.MARKER_CROSS, 10)
        return out

    def drawTimeMatches(self, alpha=0.5, prev_color=(255, 0, 0),
                        cur_color=(0, 0, 255), line_color=(255, 0, 0)):
        if (self.__frame is None or
            self.__prev_frame is None or
                self.__time_matches is None):
            return None
        out_frame_left = self.__drawTimeMatches(self.__frame.left,
                                                self.__prev_frame.left,
                                                self.__time_matches[0],
                                                alpha, prev_color,
                                                cur_color, line_color)
        out_frame_right = self.__drawTimeMatches(self.__frame.right,
                                                 self.__prev_frame.right,
                                                 self.__time_matches[1],
                                                 alpha, prev_color,
                                                 cur_color, line_color)
        out_frame = cv2.vconcat([out_frame_left, out_frame_right])
        return out_frame

    @staticmethod
    def formatRT(rvec, tvec):
        theta = np.linalg.norm(rvec)
        axis = rvec / theta
        theta_deg = theta * (180 / math.pi)
        return (("R: %.2f deg CCW around <% .3f, % .3f, % .3f>\t" +
                "T: <% .3f, % .3f, % .3f>")
                % (theta_deg, axis[0], axis[1], axis[2],
                   tvec[0], tvec[1], tvec[2]))

    @staticmethod
    def computeError(rvec, tvec, rvec_real, tvec_real):
        rmat_real = cv2.Rodrigues(rvec_real)[0]
        rmat_est = cv2.Rodrigues(rvec)[0]
        t_error = np.linalg.norm(tvec_real - tvec)
        r_error = np.linalg.norm(cv2.Rodrigues(rmat_real.dot(rmat_est.T))[0])
        return r_error, t_error

    @staticmethod
    def formatError(rvec, tvec, rvec_real, tvec_real):
        r_error, t_error = VisualOdometry.computeError(rvec, tvec,
                                                       rvec_real, tvec_real)
        r_error_deg = r_error * (180 / math.pi)
        return ("R: %.4f rad (%.4f deg)\tT: %.4f m" %
                (r_error, r_error_deg, t_error))

    def compute(self, next_frame: vi.StereoFrame, thresh=0.5):
        if next_frame is None:
            raise ValueError("next_frame cannot be empty")

        # advance stored frames
        self.__prev_frame = self.__frame
        self.__frame = next_frame

        # match points between frames, and take only those whose distance is
        # not within the given threshold (to avoid noise)
        lkp, rkp, ldesc, rdesc = self.__detectAndCompute()
        matches = self.__matcher.knnMatch(ldesc, rdesc, k=2)
        good_matches = self.__filterMatches(matches, thresh)

        # get the points corresponding to good matches.
        lPoints, rPoints = self.__getMatchPoints(lkp, rkp, good_matches)

        # get matches across the two points in time by tracking the previous
        # feature points to the current frame.
        left_matches, right_matches = self.__trackPrevToCurrent(
            lPoints, rPoints)
        self.__time_matches = (left_matches, right_matches)

        try:
            # Triangulate matching points to get distance information.
            cur_points4D = np.transpose(
                cv2.triangulatePoints(self.__P1, self.__P2,
                                      np.transpose(left_matches[1]),
                                      np.transpose(right_matches[1])))
            cur_points3D = cv2.convertPointsFromHomogeneous(cur_points4D)
            # Use solvePnP to find a rigid transform between the previous and
            # current sets of 3D points by using the 2D feature points in the
            # previous frame.
            _, rvec, tvec, _ = cv2.solvePnPRansac(cur_points3D,
                                                  np.array(left_matches[0]),
                                                  self.__P1[0:3, 0:3], None)
            tvec = tvec.reshape(3)
            return rvec, tvec
        except cv2.error:
            return None, None