Hierarchical segment-based map optimization for localization and mapping system
The disclosure provides techniques for map optimization for a localization and mapping system. The map optimization method includes segmenting, based on a preset segmentation condition, a trajectory tracked by the localization and mapping system to obtain a plurality of segments of the trajectory, each segment being partitioned into a head part, an interior part and a tail part; performing a global optimization process based on frames in the head and tail parts of each segment to obtain optimized mapping results for the frames in the head and tail parts of the segment; estimating optimized mapping results for frames in the interior part of each segment based on the optimized mapping results for the frames in the head and tail parts of the segment; and updating a map built by the localization and mapping system according to the optimized mapping results for the frames in each segment.
1 . A map optimization apparatus for a localization and mapping system, comprising interface circuitry; processor circuitry coupled to the interface circuitry and configured to:
segment, based on a preset segmentation condition, a trajectory retrieved via the interface circuitry from the localization and mapping system, to obtain a plurality of segments of the trajectory, each of the plurality of segments being partitioned into a head part, an interior part and a tail part;
perform a global optimization process based on frames in the head part and the tail part of each segment of the trajectory to obtain optimized mapping results for the frames in the head part and the tail part of each segment of the trajectory;
estimate optimized mapping results for frames in the interior part of each segment of the trajectory based on the optimized mapping results for the frames in the head part and the tail part of the segment; and
update a map built by the localization and mapping system according to the optimized mapping results for the frames in each segment of the trajectory.
2 . The map optimization apparatus of claim 1 , wherein the preset segmentation condition comprises: a modulus of a difference between a pose transformation velocity vector of a current frame in the trajectory and an average of pose transformation velocity vectors of frames in a segment preceding the current frame is less than a pose transformation velocity threshold, and a reprojection error of the current frame is less than a reprojection error threshold, and
wherein when the preset segmentation condition is satisfied, the segment preceding the current frame is extended to include the current frame, otherwise, the segment preceding the current frame ends and a new segment is created by including the current frame as a first frame of the new segment.
3 . The map optimization apparatus of claim 1 , wherein the processor circuitry is further configured to:
determine a buffer area between adjacent segments of the plurality of segments based on a preset buffer condition;
perform the global optimization process based on the frames in the head part and the tail part of each segment of the trajectory and frames in buffer areas of the trajectory to obtain the optimized mapping results for the frames in the head part and the tail part of each segment of the trajectory and optimized mapping results for the frames in the buffer areas of the trajectory; and
update the map with the optimized mapping results for the frames in each segment of the trajectory and the optimized mapping results for the frames in the buffer areas of the trajectory.
4 . The map optimization apparatus of claim 3 , wherein the preset segmentation condition comprises: a modulus of a difference between a pose transformation velocity vector of a current frame in the trajectory and an average of pose transformation velocity vectors of frames in a segment preceding the current frame is less than a pose transformation velocity threshold, and a reprojection error of the current frame is less than a reprojection error threshold, and
wherein when the preset segmentation condition is satisfied, the segment preceding the current frame is extended to include the current frame, otherwise, the segment preceding the current frame ends and the current frame is categorized as a frame of the buffer area or a new segment based on the preset buffer condition.
5 . The map optimization apparatus of claim 1 , wherein the global optimization process is a pose graph optimization process based on an optimization graph comprising pose nodes that represent poses of the frames in the trajectory and edges that represent relative pose transformations between the pose nodes.
6 . The map optimization apparatus of claim 5 , wherein the optimization graph is simplified by: deleting all pose nodes corresponding to the frames in the interior part of each segment of the trajectory and edges associated with the deleted nodes, and adding edges by connecting pose nodes corresponding to the frames in the head part and the tail part of each segment of the trajectory.
7 . The map optimization apparatus of claim 5 , wherein the optimized mapping results for the frames in each segment of the trajectory comprise optimized poses of the frames in the segment.
8 . The map optimization apparatus of claim 1 , wherein the global optimization process is a bundle adjustment (BA) optimization process based on an optimization graph comprising pose nodes that represent poses of the frames in the trajectory, landmark nodes that represent three dimensional (3D) coordinates of landmarks in the trajectory, and edges that connect landmark nodes to respective pose nodes and represent estimations of reprojection errors between the landmark nodes and the respective pose nodes.
9 . The map optimization apparatus of claim 8 , wherein the optimization graph is simplified by: selecting, under the premise of ensuring connectivity of the optimization graph, a minimum number of interconnected frames from the frames in the interior part of each segment as one or more connecting frames between the head part and the tail part of each segment, and deleting, from the interior part of each segment, landmark nodes, pose nodes and edges other than those associated with the connecting frames.
10 . The map optimization apparatus of claim 1 , wherein the processor circuitry is configured to estimate the optimized mapping results for the frames in the interior part of each segment of the trajectory by:
performing, based on the optimized mapping results for the frames in the head part and the tail part of the segment, a quaternion spherical interpolation and a linear interpolation iteratively to obtain the optimized mapping results for the frames in the interior part of the segment.
11 . The map optimization apparatus of claim 9 , wherein the processor circuitry is configured to estimate the optimized mapping results for the frames in the interior part of each segment of the trajectory by:
performing, based on the optimized mapping results for the frames in the head part and the tail part of the segment and the connecting frames, a quaternion spherical interpolation and a linear interpolation iteratively to obtain the optimized mapping results for the frames in the interior part of the segment.
12 . The map optimization apparatus of claim 1 , wherein the localization and mapping system is a Simultaneous Localization and Mapping (SLAM) system or a Structure from Motion (SfM) system.
13 . A map optimization method for a localization and mapping system, comprising:
segmenting, based on a preset segmentation condition, a trajectory tracked by the localization and mapping system to obtain a plurality of segments of the trajectory, each of the plurality of segments being partitioned into a head part, an interior part and a tail part;
performing a global optimization process based on frames in the head part and the tail part of each segment of the trajectory to obtain optimized mapping results for the frames in the head part and the tail part of each segment of the trajectory;
estimating optimized mapping results for frames in the interior part of each segment of the trajectory based on the optimized mapping results for the frames in the head part and the tail part of the segment; and
updating a map built by the localization and mapping system according to the optimized mapping results for the frames in each segment of the trajectory.
14 . The map optimization method of claim 13 , wherein the preset segmentation condition comprises: a modulus of a difference between a pose transformation velocity vector of a current frame in the trajectory and an average of pose transformation velocity vectors of frames in a segment preceding the current frame is less than a pose transformation velocity threshold, and a reprojection error of the current frame is less than a reprojection error threshold, and
wherein when the preset segmentation condition is satisfied, the segment preceding the current frame is extended to include the current frame, otherwise, the segment preceding the current frame ends and a new segment is created by including the current frame as a first frame of the new segment.
15 . The map optimization method of claim 13 , comprising:
determining a buffer area between adjacent segments of the plurality of segments based on a preset buffer condition;
performing the global optimization process based on the frames in the head part and the tail part of each segment of the trajectory and frames in buffer areas of the trajectory to obtain the optimized mapping results for the frames in the head part and the tail part of each segment of the trajectory and optimized mapping results for the frames in the buffer areas of the trajectory; and
updating the map with the optimized mapping results for the frames in each segment of the trajectory and the optimized mapping results for the frames in the buffer areas of the trajectory.
16 . The map optimization method of claim 15 , wherein the preset segmentation condition comprises: a modulus of a difference between a pose transformation velocity vector of a current frame in the trajectory and an average of pose transformation velocity vectors of frames in a segment preceding the current frame is less than a pose transformation velocity threshold, and a reprojection error of the current frame is less than a reprojection error threshold, and
wherein when the preset segmentation condition is satisfied, the segment preceding the current frame is extended to include the current frame, otherwise, the segment preceding the current frame ends and the current frame is categorized as a frame of the buffer area or a new segment based on the preset buffer condition.
17 . The map optimization method of claim 13 , wherein the global optimization process is a pose graph optimization process based on an optimization graph comprising pose nodes that represent poses of the frames in the trajectory and edges that represent relative pose transformations between the pose nodes.
18 . The map optimization method of claim 17 , wherein the optimization graph is simplified by: deleting all pose nodes corresponding to the frames in the interior part of each segment of the trajectory and edges associated with the deleted nodes, and adding edges by connecting pose nodes corresponding to the frames in the head part and the tail part of each segment of the trajectory.
19 . The map optimization method of claim 13 , wherein the global optimization process is a bundle adjustment (BA) optimization process based on an optimization graph comprising pose nodes that represent poses of the frames in the trajectory, landmark nodes that represent three dimensional (3D) coordinates of landmarks in the trajectory, and edges that connect landmark nodes to respective pose nodes and represent estimations of reprojection errors between the landmark nodes and the respective pose nodes.
20 . A non-transitory computer-readable medium comprising instructions stored thereon, wherein the instructions cause the processor circuitry to:
segment, based on a preset segmentation condition, a trajectory tracked by the localization and mapping system to obtain a plurality of segments of the trajectory, each of the plurality of segments being partitioned into a head part, an interior part and a tail part;
perform a global optimization process based on frames in the head part and the tail part of each segment of the trajectory to obtain optimized mapping results for the frames in the head part and the tail part of each segment of the trajectory;
estimate optimized mapping results for frames in the interior part of each segment of the trajectory based on the optimized mapping results for the frames in the head part and the tail part of the segment; and
update a map built by the localization and mapping system according to the optimized mapping results for the frames in each segment of the trajectory.