00001 /* 00002 * Copyright 2016 The Cartographer Authors 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 #include "cartographer/mapping/pose_graph_trimmer.h" 00018 00019 #include "glog/logging.h" 00020 00021 namespace cartographer { 00022 namespace mapping { 00023 00024 PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id, 00025 const int num_submaps_to_keep) 00026 : trajectory_id_(trajectory_id), num_submaps_to_keep_(num_submaps_to_keep) { 00027 CHECK_GE(num_submaps_to_keep, 2) << "Cannot trim with less than 2 submaps"; 00028 } 00029 00030 void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) { 00031 if (pose_graph->IsFinished(trajectory_id_)) { 00032 num_submaps_to_keep_ = 0; 00033 } 00034 00035 auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_); 00036 for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) { 00037 pose_graph->TrimSubmap(submap_ids.at(i)); 00038 } 00039 00040 if (num_submaps_to_keep_ == 0) { 00041 finished_ = true; 00042 pose_graph->SetTrajectoryState( 00043 trajectory_id_, PoseGraphInterface::TrajectoryState::DELETED); 00044 } 00045 } 00046 00047 bool PureLocalizationTrimmer::IsFinished() { return finished_; } 00048 00049 } // namespace mapping 00050 } // namespace cartographer