def Fivepoint_f2f_track(self): self._f2ftrack = [] newFrame = Frame(self._measure._pose_id) # 根据前后帧数据关联,求出当前帧位姿的初始估计 n = 0 i = 0 self._coefficient = [[],[]] while n != 5: if self._measure._data[2][i] in self._lastframe._seeDescriptor: self._coefficient[1].append((self._mappoints_DB[self._measure._data[2][i]])._pose[0][0]) self._coefficient[1].append((self._mappoints_DB[self._measure._data[2][i]])._pose[1][0]) self._coefficient[0].append(self._measure._data[0][i]) self._coefficient[0].append(self._measure._data[1][i]) n = n + 1 self._f2ftrack.append(self._measure._data[2][i]) i = i + 1 init_gs0 = np.array([[self._lastframe._pose[0][0]], [self._lastframe._pose[1][0]], [cos(self._lastframe._pose[2][0])], [sin(self._lastframe._pose[2][0])]]) init_gs = np.array([[self._lastframe._pose[0][0]],[self._lastframe._pose[1][0]],[self._lastframe._pose[2][0]]]) # 高斯牛顿法求解 GNsolve0 = Gauss_newton(self._coefficient, init_gs0) GNsolve = Gauss_newton(self._coefficient, init_gs) x = GNsolve.Solve() newFrame.set_pose(x) # 根据当前帧的位置,来估计新增加mappoint的初始位置;老的mappoints位置不变 for i in range(0, len(self._measure._data[0])): raw_measure = np.array([[self._measure._data[0][i]],[self._measure._data[1][i]]]) if self._measure._data[2][i] in self._mappoints_DB: newFrame.add_mappoint(self._mappoints_DB[self._measure._data[2][i]]) newFrame.add_measure(raw_measure, self._measure._data[2][i]) self._mappoints_DB[self._measure._data[2][i]].add_frame(newFrame) continue else: pose = np.dot(np.linalg.inv(newFrame._Rbm), raw_measure) + newFrame._tb newmappoint = Mappoint() newmappoint.set_descriptor(self._measure._data[2][i]) newmappoint.set_pose(pose) newmappoint.add_frame(newFrame) newFrame.add_mappoint(newmappoint) newFrame.add_newmappoints(newmappoint) newFrame.add_measure(raw_measure, newmappoint._descriptor) self._mappoints_DB[newmappoint._descriptor] = newmappoint self._frames_DB.append(newFrame) self._lastframe = newFrame
def Initialize(self, init_pose, measure): self._measure = measure newFrame = Frame(self._measure._pose_id) # 初始化第一帧位姿 newFrame.set_pose(init_pose) # 初始化地图点位置 for i in range(0, len(self._measure._data[0])): raw_measure = np.array([[self._measure._data[0][i]],[self._measure._data[1][i]]]) mp_pose = np.dot(np.linalg.inv(newFrame._Rbm), raw_measure) + newFrame._tb newmappoint = Mappoint() newmappoint.set_descriptor(self._measure._data[2][i]) newmappoint.set_pose(mp_pose) newmappoint.add_frame(newFrame) newFrame.add_mappoint(newmappoint) newFrame.add_newmappoints(newmappoint) newFrame.add_measure(raw_measure, newmappoint._descriptor) self._mappoints_DB[newmappoint._descriptor] = newmappoint self._frames_DB.append(newFrame) self._lastframe = newFrame