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/internal/motion_filter.h" 00018 00019 #include "cartographer/transform/transform.h" 00020 #include "glog/logging.h" 00021 00022 namespace cartographer { 00023 namespace mapping { 00024 00025 proto::MotionFilterOptions CreateMotionFilterOptions( 00026 common::LuaParameterDictionary* const parameter_dictionary) { 00027 proto::MotionFilterOptions options; 00028 options.set_max_time_seconds( 00029 parameter_dictionary->GetDouble("max_time_seconds")); 00030 options.set_max_distance_meters( 00031 parameter_dictionary->GetDouble("max_distance_meters")); 00032 options.set_max_angle_radians( 00033 parameter_dictionary->GetDouble("max_angle_radians")); 00034 return options; 00035 } 00036 00037 MotionFilter::MotionFilter(const proto::MotionFilterOptions& options) 00038 : options_(options) {} 00039 00040 bool MotionFilter::IsSimilar(const common::Time time, 00041 const transform::Rigid3d& pose) { 00042 LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500) 00043 << "Motion filter reduced the number of nodes to " 00044 << 100. * num_different_ / num_total_ << "%."; 00045 ++num_total_; 00046 if (num_total_ > 1 && 00047 time - last_time_ <= common::FromSeconds(options_.max_time_seconds()) && 00048 (pose.translation() - last_pose_.translation()).norm() <= 00049 options_.max_distance_meters() && 00050 transform::GetAngle(pose.inverse() * last_pose_) <= 00051 options_.max_angle_radians()) { 00052 return true; 00053 } 00054 last_time_ = time; 00055 last_pose_ = pose; 00056 ++num_different_; 00057 return false; 00058 } 00059 00060 } // namespace mapping 00061 } // namespace cartographer