LocalizationPoseBuffer index not updating

[daohu527]

Describe the bug LocalizationPoseBuffer is a circular queue. It is implemented with an array, and it is updated in a rolling cycle. But in LocalizationPoseBuffer::UpdateLidarPose both head_index_ and used_buffer_size_ have some problem.

Expected behavior In the function below, there is a problem with the used_buffer_size_ and head_index_. https://github.com/ApolloAuto/apollo/blob/266afbf68d83fa6fac7a812ff8a950223f5ab2c0/modules/localization/ndt/localization_pose_buffer.cc#L40-L64

We divide it into 2 cases to discuss.

1. used_buffer_size_ below 20

head_index_ will always 0 used_buffer_size_ will step add by 1 empty_position will step add by 1

2, used_buffer_size_ above 20

head_index_ will step add by 1, and equal with empty_position used_buffer_size_ will always 20 empty_position will step add by 1

When the index is less than 20, the index is incorrect

• OS: [ubuntu 18.04, ]
• Browser [e.g. chrome, safari]
• Version [apollo master]

[TychusLee]

lidar_pose_ is a circular queue caching a batch of pose pairs LocalizationStampedPosePair. The only entry to access lidar_pose_ is Eigen::Affine3d LocalizationPoseBuffer::UpdateOdometryPose which is used to predict a pose when an odometry pose comes. https://github.com/ApolloAuto/apollo/blob/266afbf68d83fa6fac7a812ff8a950223f5ab2c0/modules/localization/ndt/localization_pose_buffer.cc#L66-L68 This function uses all pose pairs in lidar_pose_, instead of the pose pair at head_index_, to predict. head_index_ just indicates the start of the circular queue. https://github.com/ApolloAuto/apollo/blob/266afbf68d83fa6fac7a812ff8a950223f5ab2c0/modules/localization/ndt/localization_pose_buffer.cc#L85-L86 Therefore head_index_ is 0 when used_buffer_size_ below s_buffer_size which is set to 20. Read the code Eigen::Affine3d LocalizationPoseBuffer::UpdateOdometryPose for more detail.