Adds MapBuilder and cleans up some configurations. (#23)

master
Damon Kohler 2016-09-08 16:21:25 +02:00 committed by GitHub
parent 54a7faf294
commit fc166fdefa
7 changed files with 314 additions and 1 deletions

View File

@ -28,6 +28,34 @@ google_library(mapping_global_trajectory_builder_interface
sensor_point_cloud
)
google_library(mapping_map_builder
USES_CERES
USES_EIGEN
SRCS
map_builder.cc
HDRS
map_builder.h
DEPENDS
common_lua_parameter_dictionary
common_thread_pool
mapping_2d_global_trajectory_builder
mapping_2d_local_trajectory_builder
mapping_2d_sparse_pose_graph
mapping_2d_submaps
mapping_3d_global_trajectory_builder
mapping_3d_local_trajectory_builder_options
mapping_3d_proto_local_trajectory_builder_options
mapping_3d_sparse_pose_graph
mapping_global_trajectory_builder_interface
mapping_proto_map_builder_options
mapping_sparse_pose_graph
mapping_trajectory_node
sensor_laser
sensor_voxel_filter
transform_rigid_transform
transform_transform
)
google_library(mapping_probability_values
USES_CERES
SRCS

View File

@ -0,0 +1,125 @@
/*
* 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/map_builder.h"
#include <cmath>
#include <limits>
#include <memory>
#include <unordered_map>
#include <utility>
#include "cartographer/common/make_unique.h"
#include "cartographer/mapping_2d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "glog/log_severity.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping {
proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::MapBuilderOptions options;
options.set_use_trajectory_builder_2d(
parameter_dictionary->GetBool("use_trajectory_builder_2d"));
*options.mutable_trajectory_builder_2d_options() =
mapping_2d::CreateLocalTrajectoryBuilderOptions(
parameter_dictionary->GetDictionary("trajectory_builder_2d").get());
options.set_use_trajectory_builder_3d(
parameter_dictionary->GetBool("use_trajectory_builder_3d"));
*options.mutable_trajectory_builder_3d_options() =
mapping_3d::CreateLocalTrajectoryBuilderOptions(
parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
options.set_num_background_threads(
parameter_dictionary->GetNonNegativeInt("num_background_threads"));
*options.mutable_sparse_pose_graph_options() = CreateSparsePoseGraphOptions(
parameter_dictionary->GetDictionary("sparse_pose_graph").get());
CHECK_NE(options.use_trajectory_builder_2d(),
options.use_trajectory_builder_3d());
return options;
}
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options,
std::deque<TrajectoryNode::ConstantData>* constant_data)
: options_(options),
thread_pool_(options.num_background_threads()) {
if (options.use_trajectory_builder_2d()) {
sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_, constant_data);
sparse_pose_graph_ = sparse_pose_graph_2d_.get();
}
if (options.use_trajectory_builder_3d()) {
sparse_pose_graph_3d_ = common::make_unique<mapping_3d::SparsePoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_, constant_data);
sparse_pose_graph_ = sparse_pose_graph_3d_.get();
}
}
MapBuilder::~MapBuilder() {}
int MapBuilder::AddTrajectoryBuilder() {
if (options_.use_trajectory_builder_3d()) {
trajectory_builders_.push_back(
common::make_unique<mapping_3d::GlobalTrajectoryBuilder>(
options_.trajectory_builder_3d_options(),
sparse_pose_graph_3d_.get()));
} else {
trajectory_builders_.push_back(
common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
options_.trajectory_builder_2d_options(),
sparse_pose_graph_2d_.get()));
}
const int trajectory_id = trajectory_builders_.size() - 1;
trajectory_ids_.emplace(trajectory_builders_.back()->submaps(),
trajectory_id);
return trajectory_id;
}
GlobalTrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
const int trajectory_id) const {
return trajectory_builders_.at(trajectory_id).get();
}
GlobalTrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
const Submaps* trajectory) const {
return trajectory_builders_.at(GetTrajectoryId(trajectory)).get();
}
int MapBuilder::GetTrajectoryId(const Submaps* trajectory) const {
const auto trajectory_id = trajectory_ids_.find(trajectory);
CHECK(trajectory_id != trajectory_ids_.end());
return trajectory_id->second;
}
proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() {
return ToProto(sparse_pose_graph_->GetConnectedTrajectories(),
trajectory_ids_);
}
int MapBuilder::num_trajectory_builders() const {
return trajectory_builders_.size();
}
SparsePoseGraph* MapBuilder::sparse_pose_graph() { return sparse_pose_graph_; }
} // namespace mapping
} // namespace cartographer

View File

@ -0,0 +1,91 @@
/*
* 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_MAP_BUILDER_H_
#define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_
#include <deque>
#include <memory>
#include <unordered_map>
#include <vector>
#include "Eigen/Geometry"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/proto/map_builder_options.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_2d/local_trajectory_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h"
namespace cartographer {
namespace mapping {
proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a SparsePoseGraph for loop closure.
class MapBuilder {
public:
MapBuilder(const proto::MapBuilderOptions& options,
std::deque<mapping::TrajectoryNode::ConstantData>* constant_data);
~MapBuilder();
MapBuilder(const MapBuilder&) = delete;
MapBuilder& operator=(const MapBuilder&) = delete;
// Create a new trajectory and return its index.
int AddTrajectoryBuilder();
// Returns the TrajectoryBuilder corresponding to the specified
// 'trajectory_id' or 'trajectory' pointer.
mapping::GlobalTrajectoryBuilderInterface* GetTrajectoryBuilder(
int trajectory_id) const;
mapping::GlobalTrajectoryBuilderInterface* GetTrajectoryBuilder(
const mapping::Submaps* trajectory) const;
// Returns the trajectory ID for 'trajectory'.
int GetTrajectoryId(const mapping::Submaps* trajectory) const;
// Returns the trajectory connectivity.
proto::TrajectoryConnectivity GetTrajectoryConnectivity();
int num_trajectory_builders() const;
mapping::SparsePoseGraph* sparse_pose_graph();
private:
const proto::MapBuilderOptions options_;
common::ThreadPool thread_pool_;
std::unique_ptr<mapping_2d::SparsePoseGraph> sparse_pose_graph_2d_;
std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_;
mapping::SparsePoseGraph* sparse_pose_graph_;
std::vector<std::unique_ptr<mapping::GlobalTrajectoryBuilderInterface>>
trajectory_builders_;
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_MAP_BUILDER_H_

View File

@ -12,6 +12,15 @@
# See the License for the specific language governing permissions and
# limitations under the License.
google_proto_library(mapping_proto_map_builder_options
SRCS
map_builder_options.proto
DEPENDS
mapping_2d_proto_local_trajectory_builder_options
mapping_3d_proto_local_trajectory_builder_options
mapping_proto_sparse_pose_graph_options
)
google_proto_library(mapping_proto_scan_matching_progress
SRCS
scan_matching_progress.proto

View File

@ -0,0 +1,34 @@
// 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.
syntax = "proto2";
import "cartographer/mapping/proto/sparse_pose_graph_options.proto";
import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto";
import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto";
package cartographer.mapping.proto;
message MapBuilderOptions {
optional bool use_trajectory_builder_2d = 1;
optional mapping_2d.proto.LocalTrajectoryBuilderOptions
trajectory_builder_2d_options = 2;
optional bool use_trajectory_builder_3d = 3;
optional mapping_3d.proto.LocalTrajectoryBuilderOptions
trajectory_builder_3d_options = 4;
// Number of threads to use for background computations.
optional int32 num_background_threads = 5;
optional SparsePoseGraphOptions sparse_pose_graph_options = 6;
}

View File

@ -0,0 +1,26 @@
-- 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 "trajectory_builder_2d.lua"
include "trajectory_builder_3d.lua"
include "sparse_pose_graph.lua"
MAP_BUILDER = {
use_trajectory_builder_2d = false,
trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
use_trajectory_builder_3d = false,
trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
num_background_threads = 4,
sparse_pose_graph = SPARSE_POSE_GRAPH,
}

View File

@ -12,7 +12,7 @@
-- See the License for the specific language governing permissions and
-- limitations under the License.
TRAJECTORY_BUILDER = {
TRAJECTORY_BUILDER_2D = {
expect_imu_data = true,
horizontal_laser_min_z = -0.8,
horizontal_laser_max_z = 2.,