00001 /* 00002 * Copyright 2018 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/internal/3d/local_slam_result_3d.h" 00018 00019 #include "cartographer/mapping/internal/3d/pose_graph_3d.h" 00020 00021 namespace cartographer { 00022 namespace mapping { 00023 00024 void LocalSlamResult3D::AddToTrajectoryBuilder( 00025 TrajectoryBuilderInterface* const trajectory_builder) { 00026 trajectory_builder->AddLocalSlamResultData( 00027 absl::make_unique<LocalSlamResult3D>(*this)); 00028 } 00029 00030 void LocalSlamResult3D::AddToPoseGraph(int trajectory_id, 00031 PoseGraph* pose_graph) const { 00032 DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph)); 00033 CHECK_GE(local_slam_result_data_.submaps().size(), 1); 00034 CHECK(local_slam_result_data_.submaps(0).has_submap_3d()); 00035 std::vector<std::shared_ptr<const mapping::Submap3D>> submaps; 00036 for (const auto& submap_proto : local_slam_result_data_.submaps()) { 00037 submaps.push_back(submap_controller_->UpdateSubmap(submap_proto)); 00038 } 00039 static_cast<PoseGraph3D*>(pose_graph) 00040 ->AddNode(std::make_shared<const mapping::TrajectoryNode::Data>( 00041 mapping::FromProto(local_slam_result_data_.node_data())), 00042 trajectory_id, submaps); 00043 } 00044 00045 } // namespace mapping 00046 } // namespace cartographer