diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_global_localizer.cc b/cartographer/mapping/internal/2d/scan_matching/fast_global_localizer.cc deleted file mode 100644 index 13a3ccd..0000000 --- a/cartographer/mapping/internal/2d/scan_matching/fast_global_localizer.cc +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright 2016 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. - */ - -#include "cartographer/mapping/internal/2d/scan_matching/fast_global_localizer.h" - -#include "glog/logging.h" - -namespace cartographer { -namespace mapping { -namespace scan_matching { - -bool PerformGlobalLocalization( - const float cutoff, const sensor::AdaptiveVoxelFilter& voxel_filter, - const std::vector& matchers, - const sensor::PointCloud& point_cloud, - transform::Rigid2d* const best_pose_estimate, float* const best_score) { - CHECK(best_pose_estimate != nullptr) - << "Need a non-null output_pose_estimate!"; - CHECK(best_score != nullptr) << "Need a non-null best_score!"; - *best_score = cutoff; - transform::Rigid2d pose_estimate; - const sensor::PointCloud filtered_point_cloud = - voxel_filter.Filter(point_cloud); - bool success = false; - if (matchers.empty()) { - LOG(WARNING) << "Map not yet large enough to localize in!"; - return false; - } - for (auto& matcher : matchers) { - float score = -1; - transform::Rigid2d pose_estimate; - if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score, - &pose_estimate)) { - CHECK_GT(score, *best_score) << "MatchFullSubmap lied!"; - *best_score = score; - *best_pose_estimate = pose_estimate; - success = true; - } - } - return success; -} - -} // namespace scan_matching -} // namespace mapping -} // namespace cartographer diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_global_localizer.h b/cartographer/mapping/internal/2d/scan_matching/fast_global_localizer.h deleted file mode 100644 index 26a9823..0000000 --- a/cartographer/mapping/internal/2d/scan_matching/fast_global_localizer.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright 2016 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_INTERNAL_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ - -#include - -#include "Eigen/Geometry" -#include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h" -#include "cartographer/sensor/internal/voxel_filter.h" - -namespace cartographer { -namespace mapping { -namespace scan_matching { - -// Perform global localization against the provided 'matchers'. The 'cutoff' -// specifies the minimum correlation that will be accepted. -// This function does not take ownership of the pointers passed in -// 'matchers'; they are passed as a vector of raw pointers to give maximum -// flexibility to callers. -// -// Returns true in the case of successful localization. The output parameters -// should not be trusted if the function returns false. The 'cutoff' and -// 'best_score' are in the range [0., 1.]. -bool PerformGlobalLocalization( - float cutoff, const sensor::AdaptiveVoxelFilter& voxel_filter, - const std::vector& matchers, - const sensor::PointCloud& point_cloud, - transform::Rigid2d* best_pose_estimate, float* best_score); - -} // namespace scan_matching -} // namespace mapping -} // namespace cartographer - -#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_