Minor fixes. (#303)

Extract mapping_2d::CreateLocalTrajectoryBuilderOptions() into its own file.
Loop initialization fix in the trajectory_connectivity_test.
Thread safety fixes in sparse_pose_graph.
master
Wolfgang Hess 2017-05-22 14:55:05 +02:00 committed by GitHub
parent 2d416589a7
commit 515db3b4a9
12 changed files with 116 additions and 52 deletions

View File

@ -16,7 +16,7 @@
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping_2d/local_trajectory_builder.h"
#include "cartographer/mapping_2d/local_trajectory_builder_options.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
namespace cartographer {

View File

@ -32,8 +32,9 @@ TEST(TrajectoryConnectivityTest, TransitivelyConnected) {
TrajectoryConnectivity trajectory_connectivity;
// Make sure nothing's connected until we connect some things.
for (int trajectory_a; trajectory_a < kNumTrajectories; ++trajectory_a) {
for (int trajectory_b; trajectory_b < kNumTrajectories; ++trajectory_b) {
for (int trajectory_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) {
for (int trajectory_b = 0; trajectory_b < kNumTrajectories;
++trajectory_b) {
EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a,
trajectory_b));
}

View File

@ -24,44 +24,6 @@
namespace cartographer {
namespace mapping_2d {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions options;
options.set_min_range(parameter_dictionary->GetDouble("min_range"));
options.set_max_range(parameter_dictionary->GetDouble("max_range"));
options.set_min_z(parameter_dictionary->GetDouble("min_z"));
options.set_max_z(parameter_dictionary->GetDouble("max_z"));
options.set_missing_data_ray_length(
parameter_dictionary->GetDouble("missing_data_ray_length"));
options.set_voxel_filter_size(
parameter_dictionary->GetDouble("voxel_filter_size"));
options.set_use_online_correlative_scan_matching(
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
*options.mutable_adaptive_voxel_filter_options() =
sensor::CreateAdaptiveVoxelFilterOptions(
parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
*options.mutable_real_time_correlative_scan_matcher_options() =
scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary
->GetDictionary("real_time_correlative_scan_matcher")
.get());
*options.mutable_ceres_scan_matcher_options() =
scan_matching::CreateCeresScanMatcherOptions(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_motion_filter_options() =
mapping_3d::CreateMotionFilterOptions(
parameter_dictionary->GetDictionary("motion_filter").get());
options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
options.set_num_odometry_states(
parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
CHECK_GT(options.num_odometry_states(), 0);
*options.mutable_submaps_options() = CreateSubmapsOptions(
parameter_dictionary->GetDictionary("submaps").get());
options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data"));
return options;
}
LocalTrajectoryBuilder::LocalTrajectoryBuilder(
const proto::LocalTrajectoryBuilderOptions& options)
: options_(options),

View File

@ -19,7 +19,6 @@
#include <memory>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/imu_tracker.h"
@ -36,9 +35,6 @@
namespace cartographer {
namespace mapping_2d {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* parameter_dictionary);
// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
// closure.
class LocalTrajectoryBuilder {

View File

@ -0,0 +1,67 @@
/*
* 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_2d/local_trajectory_builder_options.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/motion_filter.h"
#include "cartographer/sensor/voxel_filter.h"
namespace cartographer {
namespace mapping_2d {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions options;
options.set_min_range(parameter_dictionary->GetDouble("min_range"));
options.set_max_range(parameter_dictionary->GetDouble("max_range"));
options.set_min_z(parameter_dictionary->GetDouble("min_z"));
options.set_max_z(parameter_dictionary->GetDouble("max_z"));
options.set_missing_data_ray_length(
parameter_dictionary->GetDouble("missing_data_ray_length"));
options.set_voxel_filter_size(
parameter_dictionary->GetDouble("voxel_filter_size"));
options.set_use_online_correlative_scan_matching(
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
*options.mutable_adaptive_voxel_filter_options() =
sensor::CreateAdaptiveVoxelFilterOptions(
parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
*options.mutable_real_time_correlative_scan_matcher_options() =
scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary
->GetDictionary("real_time_correlative_scan_matcher")
.get());
*options.mutable_ceres_scan_matcher_options() =
scan_matching::CreateCeresScanMatcherOptions(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_motion_filter_options() =
mapping_3d::CreateMotionFilterOptions(
parameter_dictionary->GetDictionary("motion_filter").get());
options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
options.set_num_odometry_states(
parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
CHECK_GT(options.num_odometry_states(), 0);
*options.mutable_submaps_options() = CreateSubmapsOptions(
parameter_dictionary->GetDictionary("submaps").get());
options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data"));
return options;
}
} // namespace mapping_2d
} // namespace cartographer

View File

@ -0,0 +1,32 @@
/*
* 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_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h"
namespace cartographer {
namespace mapping_2d {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* parameter_dictionary);
} // namespace mapping_2d
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_

View File

@ -29,4 +29,3 @@ message ProbabilityGrid {
optional int32 min_x = 6;
optional int32 min_y = 7;
}

View File

@ -290,7 +290,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
void SparsePoseGraph::HandleScanQueue() {
constraint_builder_.WhenDone(
[this](const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
{
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
}
RunOptimization();
common::MutexLocker locker(&mutex_);
@ -334,8 +337,8 @@ void SparsePoseGraph::WaitForAllComputations() {
constraint_builder_.WhenDone(
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
});
locker.Await([&notification]() { return notification; });

View File

@ -199,7 +199,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>>

View File

@ -303,7 +303,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
void SparsePoseGraph::HandleScanQueue() {
constraint_builder_.WhenDone(
[this](const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
{
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
}
RunOptimization();
common::MutexLocker locker(&mutex_);
@ -347,8 +350,8 @@ void SparsePoseGraph::WaitForAllComputations() {
constraint_builder_.WhenDone(
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
});
locker.Await([&notification]() { return notification; });

View File

@ -196,7 +196,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>>

View File

@ -24,6 +24,7 @@
#include "cartographer/common/math.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "ceres/jet.h"