From 40d2a9de8c6a883daf8cf30fcb573443f0e975ee Mon Sep 17 00:00:00 2001 From: pifon Date: Wed, 21 Feb 2018 18:50:05 +0100 Subject: [PATCH] Address the comments. --- .../mapping/global_trajectory_builder.cc | 2 +- .../real_time_correlative_scan_matcher.cc | 26 ++++++++++++++ .../real_time_correlative_scan_matcher.h | 36 +++++++++++++++++++ .../mapping/trajectory_builder_interface.h | 2 +- .../real_time_correlative_scan_matcher_2d.cc | 17 --------- .../real_time_correlative_scan_matcher_2d.h | 5 +-- .../translation_delta_cost_functor_2d.h | 2 +- .../local_trajectory_builder_options_3d.cc | 2 +- 8 files changed, 67 insertions(+), 25 deletions(-) create mode 100644 cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.cc create mode 100644 cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h diff --git a/cartographer/internal/mapping/global_trajectory_builder.cc b/cartographer/internal/mapping/global_trajectory_builder.cc index 23abeefbef..6bce4f0e61 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.cc +++ b/cartographer/internal/mapping/global_trajectory_builder.cc @@ -48,7 +48,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override { CHECK(local_trajectory_builder_) - << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder2D."; + << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder."; std::unique_ptr matching_result = local_trajectory_builder_->AddRangeData( timed_point_cloud_data.time, diff --git a/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.cc new file mode 100644 index 0000000000..3709c28e58 --- /dev/null +++ b/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.cc @@ -0,0 +1,26 @@ +#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::RealTimeCorrelativeScanMatcherOptions options; + options.set_linear_search_window( + parameter_dictionary->GetDouble("linear_search_window")); + options.set_angular_search_window( + parameter_dictionary->GetDouble("angular_search_window")); + options.set_translation_delta_cost_weight( + parameter_dictionary->GetDouble("translation_delta_cost_weight")); + options.set_rotation_delta_cost_weight( + parameter_dictionary->GetDouble("rotation_delta_cost_weight")); + CHECK_GE(options.translation_delta_cost_weight(), 0.); + CHECK_GE(options.rotation_delta_cost_weight(), 0.); + return options; +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h new file mode 100644 index 0000000000..56c9f0167b --- /dev/null +++ b/cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h @@ -0,0 +1,36 @@ + +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index c82f555a0c..2cf93f70ee 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -107,7 +107,7 @@ class TrajectoryBuilderInterface { const sensor::LandmarkData& landmark_data) = 0; // Allows to directly add local SLAM results to the 'PoseGraph'. Note that it // is invalid to add local SLAM results for a trajectory that has a - // 'LocalTrajectoryBuilder2D'. + // 'LocalTrajectoryBuilder2D/3D'. virtual void AddLocalSlamResultData( std::unique_ptr local_slam_result_data) = 0; }; diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc index 4e2b0e79b7..2e93689c28 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc @@ -33,23 +33,6 @@ namespace cartographer { namespace mapping { namespace scan_matching { -proto::RealTimeCorrelativeScanMatcherOptions -CreateRealTimeCorrelativeScanMatcherOptions( - common::LuaParameterDictionary* const parameter_dictionary) { - proto::RealTimeCorrelativeScanMatcherOptions options; - options.set_linear_search_window( - parameter_dictionary->GetDouble("linear_search_window")); - options.set_angular_search_window( - parameter_dictionary->GetDouble("angular_search_window")); - options.set_translation_delta_cost_weight( - parameter_dictionary->GetDouble("translation_delta_cost_weight")); - options.set_rotation_delta_cost_weight( - parameter_dictionary->GetDouble("rotation_delta_cost_weight")); - CHECK_GE(options.translation_delta_cost_weight(), 0.); - CHECK_GE(options.rotation_delta_cost_weight(), 0.); - return options; -} - RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D( const proto::RealTimeCorrelativeScanMatcherOptions& options) : options_(options) {} diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h index 74fdc300de..884617a807 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h @@ -42,6 +42,7 @@ #include "Eigen/Core" #include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h" @@ -49,10 +50,6 @@ namespace cartographer { namespace mapping { namespace scan_matching { -proto::RealTimeCorrelativeScanMatcherOptions -CreateRealTimeCorrelativeScanMatcherOptions( - common::LuaParameterDictionary* const parameter_dictionary); - // An implementation of "Real-Time Correlative Scan Matching" by Olson. class RealTimeCorrelativeScanMatcher2D { public: diff --git a/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h b/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h index 279c6952a8..ee9bfc0e5a 100644 --- a/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h +++ b/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h @@ -43,7 +43,7 @@ class TranslationDeltaCostFunctor2D { } private: - // Constructs a new TranslationDeltaCostFunctor from the given + // Constructs a new TranslationDeltaCostFunctor2D from the given // 'target_translation' (x, y). explicit TranslationDeltaCostFunctor2D( const double scaling_factor, const Eigen::Vector2d& target_translation) diff --git a/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc b/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc index ea4fadf231..858f32d6b1 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_options_3d.cc @@ -17,7 +17,7 @@ #include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h" #include "cartographer/internal/mapping/motion_filter.h" -#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h" +#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/submap_3d.h" #include "cartographer/sensor/voxel_filter.h"