Check imu_data array size before access (#392)

master
Jihoon Lee 2017-07-06 14:32:45 +02:00 committed by Wolfgang Hess
parent 3f4d795c3e
commit 75d5b7a453
1 changed files with 6 additions and 4 deletions

View File

@ -89,10 +89,12 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
CHECK_EQ(trajectory_data.num_trimmed_nodes, node_id.node_index);
auto& node_data = node_data_.at(node_id.trajectory_id);
CHECK(!node_data.empty());
const common::Time node_time = node_data.front().time;
auto& imu_data = imu_data_.at(node_id.trajectory_id);
while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
imu_data.pop_front();
if (node_id.trajectory_id < static_cast<int>(imu_data_.size())) {
const common::Time node_time = node_data.front().time;
auto& imu_data = imu_data_.at(node_id.trajectory_id);
while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
imu_data.pop_front();
}
}
node_data.pop_front();
++trajectory_data.num_trimmed_nodes;