12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Optional motion-filter for adding odometry data to the pose graph. (#1757) 

This introduces an option to control the amount of odometry data that
enters the pose graph by motion-filtering it. This is very useful to
bound memory when standing still for longer periods of time.

If the new option is not configured, all odometry data goes unfiltered
into the pose graph as usual.

Signed-off-by: Michael Grupp <grupp@magazino.eu>
master
Michael Grupp 2020-11-04 14:55:13 +01:00 committed by GitHub
parent 4d90a29877
commit 6715afe70c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 58 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cartographer/mapping/internal/global_trajectory_builder.cc
View File

@ -19,7 +19,9 @@
#include <memory>
#include "absl/memory/memory.h"
#include "absl/types/optional.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/internal/motion_filter.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/metrics/family_factory.h"
#include "glog/logging.h"
@ -39,11 +41,13 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
GlobalTrajectoryBuilder(
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
const int trajectory_id, PoseGraph* const pose_graph,
const LocalSlamResultCallback& local_slam_result_callback)
const LocalSlamResultCallback& local_slam_result_callback,
const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter)
: trajectory_id_(trajectory_id),
pose_graph_(pose_graph),
local_trajectory_builder_(std::move(local_trajectory_builder)),
local_slam_result_callback_(local_slam_result_callback) {}
local_slam_result_callback_(local_slam_result_callback),
pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter) {}
~GlobalTrajectoryBuilder() override {}
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
@ -97,6 +101,14 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
if (local_trajectory_builder_) {
local_trajectory_builder_->AddOdometryData(odometry_data);
}
// TODO(MichaelGrupp): Instead of having an optional filter on this level,
// odometry could be marginalized between nodes in the pose graph.
// Related issue: cartographer-project/cartographer/#1768
if (pose_graph_odometry_motion_filter_.has_value() &&
pose_graph_odometry_motion_filter_.value().IsSimilar(
odometry_data.time, odometry_data.pose)) {
return;
}
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
@ -127,6 +139,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
PoseGraph* const pose_graph_;
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
LocalSlamResultCallback local_slam_result_callback_;
absl::optional<MotionFilter> pose_graph_odometry_motion_filter_;
};
} // namespace
@ -135,22 +148,24 @@ std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback) {
local_slam_result_callback,
const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
return absl::make_unique<
GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
std::move(local_trajectory_builder), trajectory_id, pose_graph,
local_slam_result_callback);
local_slam_result_callback, pose_graph_odometry_motion_filter);
}
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback) {
local_slam_result_callback,
const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
return absl::make_unique<
GlobalTrajectoryBuilder<LocalTrajectoryBuilder3D, mapping::PoseGraph3D>>(
std::move(local_trajectory_builder), trajectory_id, pose_graph,
local_slam_result_callback);
local_slam_result_callback, pose_graph_odometry_motion_filter);
}
void GlobalTrajectoryBuilderRegisterMetrics(metrics::FamilyFactory* factory) {

6
cartographer/mapping/internal/global_trajectory_builder.h
View File

@ -34,13 +34,15 @@ std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback);
local_slam_result_callback,
const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter);
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback);
local_slam_result_callback,
const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter);
void GlobalTrajectoryBuilderRegisterMetrics(
metrics::FamilyFactory* family_factory);

14
cartographer/mapping/map_builder.cc
View File

@ -17,6 +17,7 @@
#include "cartographer/mapping/map_builder.h"
#include "absl/memory/memory.h"
#include "absl/types/optional.h"
#include "cartographer/common/time.h"
#include "cartographer/io/internal/mapping_state_serialization.h"
#include "cartographer/io/proto_stream.h"
@ -28,6 +29,7 @@
#include "cartographer/mapping/internal/3d/pose_graph_3d.h"
#include "cartographer/mapping/internal/collated_trajectory_builder.h"
#include "cartographer/mapping/internal/global_trajectory_builder.h"
#include "cartographer/mapping/internal/motion_filter.h"
#include "cartographer/sensor/internal/collator.h"
#include "cartographer/sensor/internal/trajectory_collator.h"
#include "cartographer/sensor/internal/voxel_filter.h"
@ -120,6 +122,14 @@ int MapBuilder::AddTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options,
LocalSlamResultCallback local_slam_result_callback) {
const int trajectory_id = trajectory_builders_.size();
absl::optional<MotionFilter> pose_graph_odometry_motion_filter;
if (trajectory_options.has_pose_graph_odometry_motion_filter()) {
LOG(INFO) << "Using a motion filter for adding odometry to the pose graph.";
pose_graph_odometry_motion_filter.emplace(
MotionFilter(trajectory_options.pose_graph_odometry_motion_filter()));
}
if (options_.use_trajectory_builder_3d()) {
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_3d_options()) {
@ -134,7 +144,7 @@ int MapBuilder::AddTrajectoryBuilder(
CreateGlobalTrajectoryBuilder3D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph3D*>(pose_graph_.get()),
local_slam_result_callback)));
local_slam_result_callback, pose_graph_odometry_motion_filter)));
} else {
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_2d_options()) {
@ -149,7 +159,7 @@ int MapBuilder::AddTrajectoryBuilder(
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback)));
local_slam_result_callback, pose_graph_odometry_motion_filter)));
}
MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
pose_graph_.get());

3
cartographer/mapping/proto/trajectory_builder_options.proto
View File

@ -15,6 +15,7 @@
syntax = "proto3";
import "cartographer/transform/proto/transform.proto";
import "cartographer/mapping/proto/motion_filter_options.proto";
import "cartographer/mapping/proto/local_trajectory_builder_options_2d.proto";
import "cartographer/mapping/proto/local_trajectory_builder_options_3d.proto";
@ -41,6 +42,8 @@ message TrajectoryBuilderOptions {
bool collate_fixed_frame = 7;
bool collate_landmarks = 8;
MotionFilterOptions pose_graph_odometry_motion_filter = 9;
}
message SensorId {

18
cartographer/mapping/trajectory_builder_interface.cc
View File

@ -37,6 +37,23 @@ void PopulatePureLocalizationTrimmerOptions(
options_dictionary->GetInt("max_submaps_to_keep"));
}
void PopulatePoseGraphOdometryMotionFilterOptions(
proto::TrajectoryBuilderOptions* const trajectory_builder_options,
common::LuaParameterDictionary* const parameter_dictionary) {
constexpr char kDictionaryKey[] = "pose_graph_odometry_motion_filter";
if (!parameter_dictionary->HasKey(kDictionaryKey)) return;
auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey);
auto* options =
trajectory_builder_options->mutable_pose_graph_odometry_motion_filter();
options->set_max_time_seconds(
options_dictionary->GetDouble("max_time_seconds"));
options->set_max_distance_meters(
options_dictionary->GetDouble("max_distance_meters"));
options->set_max_angle_radians(
options_dictionary->GetDouble("max_angle_radians"));
}
} // namespace
proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
@ -53,6 +70,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
options.set_collate_landmarks(
parameter_dictionary->GetBool("collate_landmarks"));
PopulatePureLocalizationTrimmerOptions(&options, parameter_dictionary);
PopulatePoseGraphOdometryMotionFilterOptions(&options, parameter_dictionary);
return options;
}