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
