cartographer: low_resolution_matcher.cc Source File

Go to the documentation of this file.
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/3d/scan_matching/low_resolution_matcher.h"
00018 
00019 namespace cartographer {
00020 namespace mapping {
00021 namespace scan_matching {
00022 
00023 std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
00024     const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) {
00025   return [=](const transform::Rigid3f& pose) {
00026     float score = 0.f;
00027     for (const sensor::RangefinderPoint& point :
00028          sensor::TransformPointCloud(*points, pose)) {
00029       // TODO(zhengj, whess): Interpolate the Grid to get better score.
00030       score += low_resolution_grid->GetProbability(
00031           low_resolution_grid->GetCellIndex(point.position));
00032     }
00033     return score / points->size();
00034   };
00035 }
00036 
00037 }  // namespace scan_matching
00038 }  // namespace mapping
00039 }  // namespace cartographer