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

Purge 'mapping_2d::'. (#927) 

[Code structure RFC](e11bca586f/text/0000-code-structure.md)
master
Alexander Belyaev 2018-02-21 19:44:53 +01:00 committed by GitHub
parent eabcab26ed
commit 94fce13f62
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
49 changed files with 594 additions and 579 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -128,13 +128,12 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
} // namespace
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
local_trajectory_builder,
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback) {
return common::make_unique<GlobalTrajectoryBuilder<
mapping_2d::LocalTrajectoryBuilder, mapping::PoseGraph2D>>(
return common::make_unique<
GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
std::move(local_trajectory_builder), trajectory_id, pose_graph,
local_slam_result_callback);
}

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

@ -19,7 +19,7 @@
#include <memory>
#include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h"
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
@ -30,8 +30,7 @@ namespace cartographer {
namespace mapping {
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
local_trajectory_builder,
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback);

62
cartographer/internal/mapping_2d/local_trajectory_builder.cc → cartographer/internal/mapping_2d/local_trajectory_builder_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h"
#include <limits>
#include <memory>
@ -23,10 +23,10 @@
#include "cartographer/sensor/range_data.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
LocalTrajectoryBuilder::LocalTrajectoryBuilder(
const mapping::proto::LocalTrajectoryBuilderOptions2D& options)
LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
const proto::LocalTrajectoryBuilderOptions2D& options)
: options_(options),
active_submaps_(options.submaps_options()),
motion_filter_(options_.motion_filter_options()),
@ -34,10 +34,10 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
options_.real_time_correlative_scan_matcher_options()),
ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {}
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
LocalTrajectoryBuilder2D::~LocalTrajectoryBuilder2D() {}
sensor::RangeData
LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter(
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
const transform::Rigid3f& transform_to_gravity_aligned_frame,
const sensor::RangeData& range_data) const {
const sensor::RangeData cropped =
@ -50,10 +50,10 @@ LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter(
sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)};
}
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
const common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::RangeData& gravity_aligned_range_data) {
std::shared_ptr<const mapping::Submap2D> matching_submap =
std::shared_ptr<const Submap2D> matching_submap =
active_submaps_.submaps().front();
// The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
@ -80,9 +80,9 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
return pose_observation;
}
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
const sensor::TimedRangeData& range_data) {
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
const common::Time time, const sensor::TimedRangeData& range_data) {
// Initialize extrapolator now if we do not ever use an IMU.
if (!options_.use_imu_data()) {
InitializeExtrapolator(time);
@ -155,8 +155,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
return nullptr;
}
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
LocalTrajectoryBuilder::AddAccumulatedRangeData(
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment) {
@ -193,8 +193,8 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
std::move(insertion_result)});
}
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
LocalTrajectoryBuilder::InsertIntoSubmap(
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_local,
const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& pose_estimate,
@ -205,9 +205,8 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
// Querying the active submaps must be done here before calling
// InsertRangeData() since the queried values are valid for next insertion.
std::vector<std::shared_ptr<const mapping::Submap2D>> insertion_submaps;
for (const std::shared_ptr<mapping::Submap2D>& submap :
active_submaps_.submaps()) {
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
for (const std::shared_ptr<Submap2D>& submap : active_submaps_.submaps()) {
insertion_submaps.push_back(submap);
}
active_submaps_.InsertRangeData(range_data_in_local);
@ -218,25 +217,24 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
return common::make_unique<InsertionResult>(InsertionResult{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time,
gravity_alignment,
filtered_gravity_aligned_point_cloud,
{}, // 'high_resolution_point_cloud' is only used in 3D.
{}, // 'low_resolution_point_cloud' is only used in 3D.
{}, // 'rotational_scan_matcher_histogram' is only used in 3D.
pose_estimate}),
std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
time,
gravity_alignment,
filtered_gravity_aligned_point_cloud,
{}, // 'high_resolution_point_cloud' is only used in 3D.
{}, // 'low_resolution_point_cloud' is only used in 3D.
{}, // 'rotational_scan_matcher_histogram' is only used in 3D.
pose_estimate}),
std::move(insertion_submaps)});
}
void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
InitializeExtrapolator(imu_data.time);
extrapolator_->AddImuData(imu_data);
}
void LocalTrajectoryBuilder::AddOdometryData(
void LocalTrajectoryBuilder2D::AddOdometryData(
const sensor::OdometryData& odometry_data) {
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator we cannot add odometry data.
@ -246,7 +244,7 @@ void LocalTrajectoryBuilder::AddOdometryData(
extrapolator_->AddOdometryData(odometry_data);
}
void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) {
void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
if (extrapolator_ != nullptr) {
return;
}
@ -255,11 +253,11 @@ void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) {
// in time and thus the last two are used.
constexpr double kExtrapolationEstimationTimeSec = 0.001;
// TODO(gaschler): Consider using InitializeWithImu as 3D does.
extrapolator_ = common::make_unique<mapping::PoseExtrapolator>(
extrapolator_ = common::make_unique<PoseExtrapolator>(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
options_.imu_gravity_time_constant());
extrapolator_->AddPose(time, transform::Rigid3d::Identity());
}
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

36
cartographer/internal/mapping_2d/local_trajectory_builder.h → cartographer/internal/mapping_2d/local_trajectory_builder_2d.h
View File

@ -23,8 +23,8 @@
#include "cartographer/internal/mapping/motion_filter.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.pb.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/scan_matching/ceres_scan_matcher_2d.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
@ -33,16 +33,16 @@
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
// without loop closure.
// TODO(gaschler): Add test for this class similar to the 3D test.
class LocalTrajectoryBuilder {
class LocalTrajectoryBuilder2D {
public:
struct InsertionResult {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
std::vector<std::shared_ptr<const mapping::Submap2D>> insertion_submaps;
std::shared_ptr<const TrajectoryNode::Data> constant_data;
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};
struct MatchingResult {
common::Time time;
@ -52,12 +52,12 @@ class LocalTrajectoryBuilder {
std::unique_ptr<const InsertionResult> insertion_result;
};
explicit LocalTrajectoryBuilder(
const mapping::proto::LocalTrajectoryBuilderOptions2D& options);
~LocalTrajectoryBuilder();
explicit LocalTrajectoryBuilder2D(
const proto::LocalTrajectoryBuilderOptions2D& options);
~LocalTrajectoryBuilder2D();
LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete;
LocalTrajectoryBuilder2D& operator=(const LocalTrajectoryBuilder2D&) = delete;
// Returns 'MatchingResult' when range data accumulation completed,
// otherwise 'nullptr'. Range data must be approximately horizontal
@ -92,21 +92,21 @@ class LocalTrajectoryBuilder {
// Lazily constructs a PoseExtrapolator.
void InitializeExtrapolator(common::Time time);
const mapping::proto::LocalTrajectoryBuilderOptions2D options_;
mapping::ActiveSubmaps2D active_submaps_;
const proto::LocalTrajectoryBuilderOptions2D options_;
ActiveSubmaps2D active_submaps_;
mapping::MotionFilter motion_filter_;
scan_matching::RealTimeCorrelativeScanMatcher
MotionFilter motion_filter_;
scan_matching::RealTimeCorrelativeScanMatcher2D
real_time_correlative_scan_matcher_;
scan_matching::CeresScanMatcher ceres_scan_matcher_;
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
std::unique_ptr<PoseExtrapolator> extrapolator_;
int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_;
};
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_

34
cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h → cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function_2d.h
View File

@ -26,22 +26,22 @@
#include "ceres/cubic_interpolation.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
// Computes a cost for matching the 'point_cloud' to the 'probability_grid' with
// a 'pose'. The cost increases when points fall into less occupied space, i.e.
// at pixels with lower values.
class OccupiedSpaceCostFunction {
class OccupiedSpaceCostFunction2D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const mapping::ProbabilityGrid& probability_grid) {
return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
const ProbabilityGrid& probability_grid) {
return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
ceres::DYNAMIC /* residuals */,
3 /* pose variables */>(
new OccupiedSpaceCostFunction(scaling_factor, point_cloud,
probability_grid),
new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud,
probability_grid),
point_cloud.size());
}
@ -55,7 +55,7 @@ class OccupiedSpaceCostFunction {
const GridArrayAdapter adapter(probability_grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
const mapping::MapLimits& limits = probability_grid_.limits();
const MapLimits& limits = probability_grid_.limits();
for (size_t i = 0; i < point_cloud_.size(); ++i) {
// Note that this is a 2D point. The third component is a scaling factor.
@ -79,13 +79,13 @@ class OccupiedSpaceCostFunction {
public:
enum { DATA_DIMENSION = 1 };
explicit GridArrayAdapter(const mapping::ProbabilityGrid& probability_grid)
explicit GridArrayAdapter(const ProbabilityGrid& probability_grid)
: probability_grid_(probability_grid) {}
void GetValue(const int row, const int column, double* const value) const {
if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
column >= NumCols() - kPadding) {
*value = mapping::kMinProbability;
*value = kMinProbability;
} else {
*value = static_cast<double>(probability_grid_.GetProbability(
Eigen::Array2i(column - kPadding, row - kPadding)));
@ -103,27 +103,27 @@ class OccupiedSpaceCostFunction {
}
private:
const mapping::ProbabilityGrid& probability_grid_;
const ProbabilityGrid& probability_grid_;
};
OccupiedSpaceCostFunction(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const mapping::ProbabilityGrid& probability_grid)
OccupiedSpaceCostFunction2D(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
probability_grid_(probability_grid) {}
OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete;
OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) =
delete;
const double scaling_factor_;
const sensor::PointCloud& point_cloud_;
const mapping::ProbabilityGrid& probability_grid_;
const ProbabilityGrid& probability_grid_;
};
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_

2
cartographer/internal/mapping_3d/local_trajectory_builder.cc
View File

@ -20,7 +20,7 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
#include "cartographer/mapping_3d/proto/submaps_options_3d.pb.h"
#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"

10
cartographer/mapping/map_builder.cc
View File

@ -25,7 +25,7 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/internal/mapping/global_trajectory_builder.h"
#include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h"
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
#include "cartographer/mapping/collated_trajectory_builder.h"
#include "cartographer/sensor/collator.h"
@ -94,12 +94,10 @@ int MapBuilder::AddTrajectoryBuilder(
trajectory_id, pose_graph_3d_.get(),
local_slam_result_callback)));
} else {
std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
local_trajectory_builder;
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder =
common::make_unique<mapping_2d::LocalTrajectoryBuilder>(
trajectory_options.trajectory_builder_2d_options());
local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(
trajectory_options.trajectory_builder_2d_options());
}
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(

8
cartographer/mapping/pose_graph/constraint_builder.cc
View File

@ -16,8 +16,8 @@
#include "cartographer/mapping/pose_graph/constraint_builder.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/sensor/voxel_filter.h"
@ -41,11 +41,11 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
parameter_dictionary->GetDouble("loop_closure_rotation_weight"));
options.set_log_matches(parameter_dictionary->GetBool("log_matches"));
*options.mutable_fast_correlative_scan_matcher_options() =
mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions(
scan_matching::CreateFastCorrelativeScanMatcherOptions2D(
parameter_dictionary->GetDictionary("fast_correlative_scan_matcher")
.get());
*options.mutable_ceres_scan_matcher_options() =
mapping_2d::scan_matching::CreateCeresScanMatcherOptions(
scan_matching::CreateCeresScanMatcherOptions2D(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_fast_correlative_scan_matcher_options_3d() =
mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions(

9
cartographer/mapping/pose_graph/proto/constraint_builder_options.proto
View File

@ -16,8 +16,8 @@ syntax = "proto3";
package cartographer.mapping.pose_graph.proto;
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto";
import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto";
import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto";
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
@ -48,13 +48,12 @@ message ConstraintBuilderOptions {
bool log_matches = 8;
// Options for the internally used scan matchers.
mapping_2d.scan_matching.proto.FastCorrelativeScanMatcherOptions
mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions2D
fast_correlative_scan_matcher_options = 9;
mapping_2d.scan_matching.proto.CeresScanMatcherOptions
mapping.scan_matching.proto.CeresScanMatcherOptions2D
ceres_scan_matcher_options = 11;
mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
fast_correlative_scan_matcher_options_3d = 10;
mapping_3d.scan_matching.proto.CeresScanMatcherOptions
ceres_scan_matcher_options_3d = 12;
}

2
cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto → cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto
View File

@ -14,7 +14,7 @@
syntax = "proto3";
package cartographer.mapping_2d.scan_matching.proto;
package cartographer.mapping.scan_matching.proto;
message RealTimeCorrelativeScanMatcherOptions {
// Minimum linear search window in which the best possible scan alignment

26
cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.cc Normal file
View File

@ -0,0 +1,26 @@
#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
proto::RealTimeCorrelativeScanMatcherOptions
CreateRealTimeCorrelativeScanMatcherOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::RealTimeCorrelativeScanMatcherOptions options;
options.set_linear_search_window(
parameter_dictionary->GetDouble("linear_search_window"));
options.set_angular_search_window(
parameter_dictionary->GetDouble("angular_search_window"));
options.set_translation_delta_cost_weight(
parameter_dictionary->GetDouble("translation_delta_cost_weight"));
options.set_rotation_delta_cost_weight(
parameter_dictionary->GetDouble("rotation_delta_cost_weight"));
CHECK_GE(options.translation_delta_cost_weight(), 0.);
CHECK_GE(options.rotation_delta_cost_weight(), 0.);
return options;
}
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer

36
cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h Normal file
View File

@ -0,0 +1,36 @@
/*
* Copyright 2018 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_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
proto::RealTimeCorrelativeScanMatcherOptions
CreateRealTimeCorrelativeScanMatcherOptions(
common::LuaParameterDictionary* const parameter_dictionary);
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_

2
cartographer/mapping/trajectory_builder_interface.h
View File

@ -107,7 +107,7 @@ class TrajectoryBuilderInterface {
const sensor::LandmarkData& landmark_data) = 0;
// Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
// is invalid to add local SLAM results for a trajectory that has a
// 'LocalTrajectoryBuilder'.
// 'LocalTrajectoryBuilder2D/3D'.
virtual void AddLocalSlamResultData(
std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;
};

8
cartographer/mapping_2d/local_trajectory_builder_options_2d.cc
View File

@ -17,8 +17,8 @@
#include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h"
#include "cartographer/internal/mapping/motion_filter.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/scan_matching/ceres_scan_matcher_2d.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/sensor/voxel_filter.h"
@ -49,12 +49,12 @@ proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D(
->GetDictionary("loop_closure_adaptive_voxel_filter")
.get());
*options.mutable_real_time_correlative_scan_matcher_options() =
mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary
->GetDictionary("real_time_correlative_scan_matcher")
.get());
*options.mutable_ceres_scan_matcher_options() =
mapping_2d::scan_matching::CreateCeresScanMatcherOptions(
mapping::scan_matching::CreateCeresScanMatcherOptions2D(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions(
parameter_dictionary->GetDictionary("motion_filter").get());

72
cartographer/mapping_2d/pose_graph/constraint_builder.cc → cartographer/mapping_2d/pose_graph/constraint_builder_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h"
#include <cmath>
#include <functional>
@ -29,8 +29,8 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.pb.h"
#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.pb.h"
#include "cartographer/metrics/counter.h"
#include "cartographer/metrics/gauge.h"
#include "cartographer/metrics/histogram.h"
@ -38,7 +38,7 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace pose_graph {
static auto* kConstraintsSearchedMetric = metrics::Counter::Null();
@ -49,19 +49,19 @@ static auto* kQueueLengthMetric = metrics::Gauge::Null();
static auto* kConstraintScoresMetric = metrics::Histogram::Null();
static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null();
transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap) {
transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) {
return transform::Project2D(submap.local_pose());
}
ConstraintBuilder::ConstraintBuilder(
const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
ConstraintBuilder2D::ConstraintBuilder2D(
const pose_graph::proto::ConstraintBuilderOptions& options,
common::ThreadPool* const thread_pool)
: options_(options),
thread_pool_(thread_pool),
sampler_(options.sampling_ratio()),
ceres_scan_matcher_(options.ceres_scan_matcher_options()) {}
ConstraintBuilder::~ConstraintBuilder() {
ConstraintBuilder2D::~ConstraintBuilder2D() {
common::MutexLocker locker(&mutex_);
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
CHECK_EQ(pending_computations_.size(), 0);
@ -69,10 +69,9 @@ ConstraintBuilder::~ConstraintBuilder() {
CHECK(when_done_ == nullptr);
}
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
void ConstraintBuilder2D::MaybeAddConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
@ -95,10 +94,9 @@ void ConstraintBuilder::MaybeAddConstraint(
}
}
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data) {
void ConstraintBuilder2D::MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data) {
common::MutexLocker locker(&mutex_);
constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size());
@ -114,13 +112,13 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
});
}
void ConstraintBuilder::NotifyEndOfNode() {
void ConstraintBuilder2D::NotifyEndOfNode() {
common::MutexLocker locker(&mutex_);
++current_computation_;
}
void ConstraintBuilder::WhenDone(
const std::function<void(const ConstraintBuilder::Result&)>& callback) {
void ConstraintBuilder2D::WhenDone(
const std::function<void(const ConstraintBuilder2D::Result&)>& callback) {
common::MutexLocker locker(&mutex_);
CHECK(when_done_ == nullptr);
when_done_ =
@ -131,9 +129,8 @@ void ConstraintBuilder::WhenDone(
[this, current_computation] { FinishComputation(current_computation); });
}
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id,
const mapping::ProbabilityGrid* const submap,
void ConstraintBuilder2D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const SubmapId& submap_id, const ProbabilityGrid* const submap,
const std::function<void()>& work_item) {
if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
nullptr) {
@ -147,11 +144,10 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
}
}
void ConstraintBuilder::ConstructSubmapScanMatcher(
const mapping::SubmapId& submap_id,
const mapping::ProbabilityGrid* const submap) {
void ConstraintBuilder2D::ConstructSubmapScanMatcher(
const SubmapId& submap_id, const ProbabilityGrid* const submap) {
auto submap_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
common::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
*submap, options_.fast_correlative_scan_matcher_options());
common::MutexLocker locker(&mutex_);
submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
@ -162,8 +158,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher(
submap_queued_work_items_.erase(submap_id);
}
const ConstraintBuilder::SubmapScanMatcher*
ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::GetSubmapScanMatcher(const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
const SubmapScanMatcher* submap_scan_matcher =
&submap_scan_matchers_[submap_id];
@ -171,12 +167,12 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
return submap_scan_matcher;
}
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap,
const mapping::NodeId& node_id, bool match_full_submap,
const mapping::TrajectoryNode::Data* const constant_data,
void ConstraintBuilder2D::ComputeConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
const SubmapScanMatcher* const submap_scan_matcher =
@ -262,7 +258,7 @@ void ConstraintBuilder::ComputeConstraint(
}
}
void ConstraintBuilder::FinishComputation(const int computation_index) {
void ConstraintBuilder2D::FinishComputation(const int computation_index) {
Result result;
std::unique_ptr<std::function<void(const Result&)>> callback;
{
@ -295,7 +291,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
}
}
int ConstraintBuilder::GetNumFinishedNodes() {
int ConstraintBuilder2D::GetNumFinishedNodes() {
common::MutexLocker locker(&mutex_);
if (pending_computations_.empty()) {
return current_computation_;
@ -303,13 +299,13 @@ int ConstraintBuilder::GetNumFinishedNodes() {
return pending_computations_.begin()->first;
}
void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
CHECK(pending_computations_.empty());
submap_scan_matchers_.erase(submap_id);
}
void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) {
void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) {
auto* counts = factory->NewCounterFamily(
"/mapping_2d/pose_graph/constraint_builder/constraints",
"Constraints computed");
@ -333,5 +329,5 @@ void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) {
}
} // namespace pose_graph
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

81
cartographer/mapping_2d/pose_graph/constraint_builder.h → cartographer/mapping_2d/pose_graph/constraint_builder_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_
#include <array>
#include <deque>
@ -32,20 +32,20 @@
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/voxel_filter.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace pose_graph {
// Returns (map <- submap) where 'submap' is a coordinate system at the origin
// of the Submap.
transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap);
transform::Rigid2d ComputeSubmapPose(const Submap2D& submap);
// Asynchronously computes constraints.
//
@ -55,18 +55,18 @@ transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap);
// MaybeAdd(Global)Constraint()/WhenDone() cycle can follow.
//
// This class is thread-safe.
class ConstraintBuilder {
class ConstraintBuilder2D {
public:
using Constraint = mapping::PoseGraphInterface::Constraint;
using Constraint = PoseGraphInterface::Constraint;
using Result = std::vector<Constraint>;
ConstraintBuilder(
const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
ConstraintBuilder2D(
const pose_graph::proto::ConstraintBuilderOptions& options,
common::ThreadPool* thread_pool);
~ConstraintBuilder();
~ConstraintBuilder2D();
ConstraintBuilder(const ConstraintBuilder&) = delete;
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
ConstraintBuilder2D(const ConstraintBuilder2D&) = delete;
ConstraintBuilder2D& operator=(const ConstraintBuilder2D&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The
@ -74,11 +74,10 @@ class ConstraintBuilder {
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap2D* submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose);
void MaybeAddConstraint(const SubmapId& submap_id, const Submap2D* submap,
const NodeId& node_id,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
@ -87,9 +86,8 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap2D* submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data);
const SubmapId& submap_id, const Submap2D* submap, const NodeId& node_id,
const TrajectoryNode::Data* const constant_data);
// Must be called after all computations related to one node have been added.
void NotifyEndOfNode();
@ -102,48 +100,47 @@ class ConstraintBuilder {
int GetNumFinishedNodes();
// Delete data related to 'submap_id'.
void DeleteScanMatcher(const mapping::SubmapId& submap_id);
void DeleteScanMatcher(const SubmapId& submap_id);
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
private:
struct SubmapScanMatcher {
const mapping::ProbabilityGrid* probability_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
const ProbabilityGrid* probability_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
fast_correlative_scan_matcher;
};
// Either schedules the 'work_item', or if needed, schedules the scan matcher
// construction and queues the 'work_item'.
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id,
const mapping::ProbabilityGrid* submap,
const SubmapId& submap_id, const ProbabilityGrid* submap,
const std::function<void()>& work_item) REQUIRES(mutex_);
// Constructs the scan matcher for a 'submap', then schedules its work items.
void ConstructSubmapScanMatcher(const mapping::SubmapId& submap_id,
const mapping::ProbabilityGrid* submap)
void ConstructSubmapScanMatcher(const SubmapId& submap_id,
const ProbabilityGrid* submap)
EXCLUDES(mutex_);
// Returns the scan matcher for a submap, which has to exist.
const SubmapScanMatcher* GetSubmapScanMatcher(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
const SubmapScanMatcher* GetSubmapScanMatcher(const SubmapId& submap_id)
EXCLUDES(mutex_);
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'compressed_point_cloud' do not change
// anymore. As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap2D* submap,
const mapping::NodeId& node_id, bool match_full_submap,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
void ComputeConstraint(const SubmapId& submap_id, const Submap2D* submap,
const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<Constraint>* constraint)
EXCLUDES(mutex_);
// Decrements the 'pending_computations_' count. If all computations are done,
// runs the 'when_done_' callback and resets the state.
void FinishComputation(int computation_index) EXCLUDES(mutex_);
const mapping::pose_graph::proto::ConstraintBuilderOptions options_;
const pose_graph::proto::ConstraintBuilderOptions options_;
common::ThreadPool* thread_pool_;
common::Mutex mutex_;
@ -165,23 +162,23 @@ class ConstraintBuilder {
std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
// Map of already constructed scan matchers by 'submap_id'.
std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_
std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
GUARDED_BY(mutex_);
// Map by 'submap_id' of scan matchers under construction, and the work
// to do once construction is done.
std::map<mapping::SubmapId, std::vector<std::function<void()>>>
std::map<SubmapId, std::vector<std::function<void()>>>
submap_queued_work_items_ GUARDED_BY(mutex_);
common::FixedRatioSampler sampler_;
scan_matching::CeresScanMatcher ceres_scan_matcher_;
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
// Histogram of scan matcher scores.
common::Histogram score_histogram_ GUARDED_BY(mutex_);
};
} // namespace pose_graph
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_

36
cartographer/mapping_2d/pose_graph/landmark_cost_function.h → cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h
View File

@ -14,49 +14,50 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/mapping/pose_graph/cost_helpers.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "ceres/jet.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace pose_graph {
// Cost function measuring the weighted error between the observed pose given by
// the landmark measurement and the linearly interpolated pose of embedded in 3D
// space node poses.
class LandmarkCostFunction {
class LandmarkCostFunction2D {
public:
using LandmarkObservation =
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
PoseGraphInterface::LandmarkNode::LandmarkObservation;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation, const NodeData& prev_node,
const NodeData& next_node) {
const LandmarkObservation& observation,
const OptimizationProblem2D::NodeData& prev_node,
const OptimizationProblem2D::NodeData& next_node) {
return new ceres::AutoDiffCostFunction<
LandmarkCostFunction, 6 /* residuals */,
LandmarkCostFunction2D, 6 /* residuals */,
3 /* previous node pose variables */, 3 /* next node pose variables */,
4 /* landmark rotation variables */,
3 /* landmark translation variables */>(
new LandmarkCostFunction(observation, prev_node, next_node));
new LandmarkCostFunction2D(observation, prev_node, next_node));
}
template <typename T>
bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
const T* const landmark_rotation,
const T* const landmark_translation, T* const e) const {
using mapping::pose_graph::ComputeUnscaledError;
using mapping::pose_graph::ScaleError;
using mapping::pose_graph::SlerpQuaternions;
using pose_graph::ComputeUnscaledError;
using pose_graph::ScaleError;
using pose_graph::SlerpQuaternions;
const std::array<T, 3> interpolated_pose_translation{
{prev_node_pose[0] +
@ -102,8 +103,9 @@ class LandmarkCostFunction {
}
private:
LandmarkCostFunction(const LandmarkObservation& observation,
const NodeData& prev_node, const NodeData& next_node)
LandmarkCostFunction2D(const LandmarkObservation& observation,
const OptimizationProblem2D::NodeData& prev_node,
const OptimizationProblem2D::NodeData& next_node)
: landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform),
prev_node_gravity_alignment_(prev_node.gravity_alignment),
@ -123,7 +125,7 @@ class LandmarkCostFunction {
};
} // namespace pose_graph
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_

14
cartographer/mapping_2d/pose_graph/landmark_cost_function_test.cc → cartographer/mapping_2d/pose_graph/landmark_cost_function_2d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h"
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h"
#include <memory>
@ -23,7 +23,7 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace pose_graph {
namespace {
@ -31,18 +31,18 @@ using ::testing::DoubleEq;
using ::testing::ElementsAre;
using LandmarkObservation =
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
PoseGraphInterface::LandmarkNode::LandmarkObservation;
TEST(LandmarkCostFunctionTest, SmokeTest) {
NodeData prev_node;
OptimizationProblem2D::NodeData prev_node;
prev_node.time = common::FromUniversal(0);
prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
NodeData next_node;
OptimizationProblem2D::NodeData next_node;
next_node.time = common::FromUniversal(10);
next_node.gravity_alignment = Eigen::Quaterniond::Identity();
std::unique_ptr<ceres::CostFunction> cost_function(
LandmarkCostFunction::CreateAutoDiffCostFunction(
LandmarkCostFunction2D::CreateAutoDiffCostFunction(
LandmarkObservation{
0 /* trajectory ID */,
common::FromUniversal(5) /* time */,
@ -73,5 +73,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
} // namespace
} // namespace pose_graph
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

82
cartographer/mapping_2d/pose_graph/optimization_problem.cc → cartographer/mapping_2d/pose_graph/optimization_problem_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h"
#include <algorithm>
#include <array>
@ -28,21 +28,22 @@
#include "cartographer/common/histogram.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/pose_graph/ceres_pose.h"
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h"
#include "cartographer/mapping_2d/pose_graph/spa_cost_function.h"
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h"
#include "cartographer/mapping_2d/pose_graph/spa_cost_function_2d.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace pose_graph {
namespace {
using ::cartographer::mapping::pose_graph::CeresPose;
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
using NodeData = OptimizationProblem2D::NodeData;
using SubmapData = OptimizationProblem2D::SubmapData;
// Converts a pose into the 3 optimization variable format used for Ceres:
// translation in x and y, followed by the rotation angle representing the
@ -73,8 +74,8 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes,
const mapping::MapById<mapping::NodeId, NodeData>& node_data,
mapping::MapById<mapping::NodeId, std::array<double, 3>>* C_nodes,
const MapById<NodeId, NodeData>& node_data,
MapById<NodeId, std::array<double, 3>>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
for (const auto& landmark_node : landmark_nodes) {
for (const auto& observation : landmark_node.second.landmark_observations) {
@ -110,7 +111,7 @@ void AddLandmarkCostFunctions(
problem));
}
problem->AddResidualBlock(
LandmarkCostFunction::CreateAutoDiffCostFunction(
LandmarkCostFunction2D::CreateAutoDiffCostFunction(
observation, prev->data, next->data),
nullptr /* loss function */, C_nodes->at(prev->id).data(),
C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(),
@ -121,23 +122,23 @@ void AddLandmarkCostFunctions(
} // namespace
OptimizationProblem::OptimizationProblem(
const mapping::pose_graph::proto::OptimizationProblemOptions& options)
OptimizationProblem2D::OptimizationProblem2D(
const pose_graph::proto::OptimizationProblemOptions& options)
: options_(options) {}
OptimizationProblem::~OptimizationProblem() {}
OptimizationProblem2D::~OptimizationProblem2D() {}
void OptimizationProblem::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
void OptimizationProblem2D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
imu_data_.Append(trajectory_id, imu_data);
}
void OptimizationProblem::AddOdometryData(
void OptimizationProblem2D::AddOdometryData(
const int trajectory_id, const sensor::OdometryData& odometry_data) {
odometry_data_.Append(trajectory_id, odometry_data);
}
void OptimizationProblem::AddTrajectoryNode(
void OptimizationProblem2D::AddTrajectoryNode(
const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment) {
@ -145,41 +146,41 @@ void OptimizationProblem::AddTrajectoryNode(
NodeData{time, initial_pose, pose, gravity_alignment});
}
void OptimizationProblem::InsertTrajectoryNode(
const mapping::NodeId& node_id, const common::Time time,
void OptimizationProblem2D::InsertTrajectoryNode(
const NodeId& node_id, const common::Time time,
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment) {
node_data_.Insert(node_id,
NodeData{time, initial_pose, pose, gravity_alignment});
}
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
imu_data_.Trim(node_data_, node_id);
odometry_data_.Trim(node_data_, node_id);
node_data_.Trim(node_id);
}
void OptimizationProblem::AddSubmap(
void OptimizationProblem2D::AddSubmap(
const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
}
void OptimizationProblem::InsertSubmap(
const mapping::SubmapId& submap_id,
const transform::Rigid2d& global_submap_pose) {
void OptimizationProblem2D::InsertSubmap(
const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
}
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
void OptimizationProblem2D::TrimSubmap(const SubmapId& submap_id) {
submap_data_.Trim(submap_id);
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
void OptimizationProblem2D::SetMaxNumIterations(
const int32 max_num_iterations) {
options_.mutable_ceres_solver_options()->set_max_num_iterations(
max_num_iterations);
}
void OptimizationProblem::Solve(
void OptimizationProblem2D::Solve(
const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes) {
@ -193,8 +194,8 @@ void OptimizationProblem::Solve(
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData.
mapping::MapById<mapping::SubmapId, std::array<double, 3>> C_submaps;
mapping::MapById<mapping::NodeId, std::array<double, 3>> C_nodes;
MapById<SubmapId, std::array<double, 3>> C_submaps;
MapById<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true;
for (const auto& submap_id_data : submap_data_) {
@ -222,7 +223,7 @@ void OptimizationProblem::Solve(
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose),
SpaCostFunction2D::CreateAutoDiffCostFunction(constraint.pose),
// Only loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
@ -245,10 +246,10 @@ void OptimizationProblem::Solve(
auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) {
const mapping::NodeId first_node_id = prev_node_it->id;
const NodeId first_node_id = prev_node_it->id;
const NodeData& first_node_data = prev_node_it->data;
prev_node_it = node_it;
const mapping::NodeId second_node_id = node_it->id;
const NodeId second_node_id = node_it->id;
const NodeData& second_node_data = node_it->data;
if (second_node_id.node_index != first_node_id.node_index + 1) {
@ -258,7 +259,7 @@ void OptimizationProblem::Solve(
const transform::Rigid3d relative_pose =
ComputeRelativePose(trajectory_id, first_node_data, second_node_data);
problem.AddResidualBlock(
SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{
SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{
relative_pose, options_.consecutive_node_translation_weight(),
options_.consecutive_node_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
@ -288,32 +289,31 @@ void OptimizationProblem::Solve(
}
}
const mapping::MapById<mapping::NodeId, NodeData>&
OptimizationProblem::node_data() const {
const MapById<NodeId, NodeData>& OptimizationProblem2D::node_data() const {
return node_data_;
}
const mapping::MapById<mapping::SubmapId, SubmapData>&
OptimizationProblem::submap_data() const {
const MapById<SubmapId, SubmapData>& OptimizationProblem2D::submap_data()
const {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>&
OptimizationProblem::landmark_data() const {
OptimizationProblem2D::landmark_data() const {
return landmark_data_;
}
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem2D::imu_data()
const {
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>&
OptimizationProblem::odometry_data() const {
OptimizationProblem2D::odometry_data() const {
return odometry_data_;
}
std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
const int trajectory_id, const common::Time time) const {
const auto it = odometry_data_.lower_bound(trajectory_id, time);
if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
@ -332,7 +332,7 @@ std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
.transform);
}
transform::Rigid3d OptimizationProblem::ComputeRelativePose(
transform::Rigid3d OptimizationProblem2D::ComputeRelativePose(
const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) {
@ -352,5 +352,5 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose(
}
} // namespace pose_graph
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

66
cartographer/mapping_2d/pose_graph/optimization_problem.h → cartographer/mapping_2d/pose_graph/optimization_problem_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
#include <array>
#include <deque>
@ -36,32 +36,32 @@
#include "cartographer/transform/timestamped_transform.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace pose_graph {
struct NodeData {
common::Time time;
transform::Rigid2d initial_pose;
transform::Rigid2d pose;
Eigen::Quaterniond gravity_alignment;
};
struct SubmapData {
transform::Rigid2d global_pose;
};
// Implements the SPA loop closure method.
class OptimizationProblem {
class OptimizationProblem2D {
public:
using Constraint = mapping::PoseGraphInterface::Constraint;
using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode;
using Constraint = PoseGraphInterface::Constraint;
using LandmarkNode = PoseGraphInterface::LandmarkNode;
explicit OptimizationProblem(
const mapping::pose_graph::proto::OptimizationProblemOptions& options);
~OptimizationProblem();
struct NodeData {
common::Time time;
transform::Rigid2d initial_pose;
transform::Rigid2d pose;
Eigen::Quaterniond gravity_alignment;
};
OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
struct SubmapData {
transform::Rigid2d global_pose;
};
explicit OptimizationProblem2D(
const pose_graph::proto::OptimizationProblemOptions& options);
~OptimizationProblem2D();
OptimizationProblem2D(const OptimizationProblem2D&) = delete;
OptimizationProblem2D& operator=(const OptimizationProblem2D&) = delete;
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddOdometryData(int trajectory_id,
@ -70,16 +70,16 @@ class OptimizationProblem {
const transform::Rigid2d& initial_pose,
const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment);
void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
const transform::Rigid2d& initial_pose,
const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void TrimTrajectoryNode(const NodeId& node_id);
void AddSubmap(int trajectory_id,
const transform::Rigid2d& global_submap_pose);
void InsertSubmap(const mapping::SubmapId& submap_id,
void InsertSubmap(const SubmapId& submap_id,
const transform::Rigid2d& global_submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);
void TrimSubmap(const SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);
@ -88,8 +88,8 @@ class OptimizationProblem {
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes);
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
const MapById<NodeId, NodeData>& node_data() const;
const MapById<SubmapId, SubmapData>& submap_data() const;
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
@ -102,16 +102,16 @@ class OptimizationProblem {
int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const;
mapping::pose_graph::proto::OptimizationProblemOptions options_;
mapping::MapById<mapping::NodeId, NodeData> node_data_;
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
pose_graph::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeData> node_data_;
MapById<SubmapId, SubmapData> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
};
} // namespace pose_graph
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_

29
cartographer/mapping_2d/pose_graph/spa_cost_function.h → cartographer/mapping_2d/pose_graph/spa_cost_function_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
#include <array>
@ -30,25 +30,23 @@
#include "ceres/jet.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace pose_graph {
class SpaCostFunction {
class SpaCostFunction2D {
public:
using Constraint = mapping::PoseGraph::Constraint;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const Constraint::Pose& pose) {
return new ceres::AutoDiffCostFunction<SpaCostFunction, 3 /* residuals */,
const PoseGraph::Constraint::Pose& pose) {
return new ceres::AutoDiffCostFunction<SpaCostFunction2D, 3 /* residuals */,
3 /* pose variables */,
3 /* pose variables */>(
new SpaCostFunction(pose));
new SpaCostFunction2D(pose));
}
template <typename T>
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
using mapping::pose_graph::ComputeUnscaledError;
using mapping::pose_graph::ScaleError;
using pose_graph::ComputeUnscaledError;
using pose_graph::ScaleError;
const std::array<T, 3> error = ScaleError(
ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
@ -58,13 +56,14 @@ class SpaCostFunction {
}
private:
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
explicit SpaCostFunction2D(const PoseGraph::Constraint::Pose& pose)
: pose_(pose) {}
const Constraint::Pose pose_;
const PoseGraph::Constraint::Pose pose_;
};
} // namespace pose_graph
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_

23
cartographer/mapping_2d/pose_graph_2d.cc
View File

@ -86,9 +86,8 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
optimization_problem_.AddSubmap(
trajectory_id,
first_submap_pose *
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[0])
.inverse() *
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
return {last_submap_id,
SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
}
@ -244,8 +243,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data().at(matching_id).global_pose *
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() *
pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
pose;
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
@ -257,9 +255,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform =
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[i])
.inverse() *
pose;
pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose;
constraints_.push_back(Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
@ -325,7 +321,7 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
void PoseGraph2D::HandleWorkQueue() {
constraint_builder_.WhenDone(
[this](const mapping_2d::pose_graph::ConstraintBuilder::Result& result) {
[this](const pose_graph::ConstraintBuilder2D::Result& result) {
{
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
@ -386,8 +382,8 @@ void PoseGraph2D::WaitForAllComputations() {
}
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone(
[this, &notification](
const mapping_2d::pose_graph::ConstraintBuilder::Result& result) {
[this,
&notification](const pose_graph::ConstraintBuilder2D::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
@ -440,7 +436,8 @@ void PoseGraph2D::AddSubmapFromProto(
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert(
submap_id, mapping_2d::pose_graph::SubmapData{global_submap_pose_2d});
submap_id,
pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
@ -728,7 +725,7 @@ PoseGraph2D::GetAllSubmapPoses() {
}
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, mapping_2d::pose_graph::SubmapData>&
const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
global_submap_poses,
const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);

15
cartographer/mapping_2d/pose_graph_2d.h
View File

@ -35,8 +35,8 @@
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/trajectory_connectivity_state.h"
#include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h"
#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h"
@ -190,7 +190,7 @@ class PoseGraph2D : public PoseGraph {
// Computes the local to global map frame transform based on the given
// 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, mapping_2d::pose_graph::SubmapData>&
const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
global_submap_poses,
int trajectory_id) const REQUIRES(mutex_);
@ -230,9 +230,8 @@ class PoseGraph2D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem.
mapping_2d::pose_graph::OptimizationProblem optimization_problem_;
mapping_2d::pose_graph::ConstraintBuilder constraint_builder_
GUARDED_BY(mutex_);
pose_graph::OptimizationProblem2D optimization_problem_;
pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
// Submaps get assigned an ID and state as soon as they are seen, even
@ -244,8 +243,8 @@ class PoseGraph2D : public PoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data.
MapById<SubmapId, mapping_2d::pose_graph::SubmapData> global_submap_poses_
GUARDED_BY(mutex_);
MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>
global_submap_poses_ GUARDED_BY(mutex_);
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>

11
cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto
View File

@ -19,8 +19,8 @@ package cartographer.mapping.proto;
import "cartographer/mapping/proto/motion_filter_options.proto";
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
import "cartographer/mapping_2d/proto/submaps_options_2d.proto";
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto";
import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto";
import "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
message LocalTrajectoryBuilderOptions2D {
// Rangefinder points outside these ranges will be dropped.
@ -51,10 +51,9 @@ message LocalTrajectoryBuilderOptions2D {
// Whether to solve the online scan matching first using the correlative scan
// matcher to generate a good starting point for Ceres.
bool use_online_correlative_scan_matching = 5;
cartographer.mapping_2d.scan_matching.proto
.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 7;
cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions
cartographer.mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 7;
cartographer.mapping.scan_matching.proto.CeresScanMatcherOptions2D
ceres_scan_matcher_options = 8;
MotionFilterOptions motion_filter_options = 13;

40
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc → cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
#include <utility>
#include <vector>
@ -22,21 +22,21 @@
#include "Eigen/Core"
#include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h"
#include "cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function_2d.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h"
#include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h"
#include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor_2d.h"
#include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::CeresScanMatcherOptions options;
proto::CeresScanMatcherOptions2D options;
options.set_occupied_space_weight(
parameter_dictionary->GetDouble("occupied_space_weight"));
options.set_translation_weight(
@ -49,41 +49,41 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
return options;
}
CeresScanMatcher::CeresScanMatcher(
const proto::CeresScanMatcherOptions& options)
CeresScanMatcher2D::CeresScanMatcher2D(
const proto::CeresScanMatcherOptions2D& options)
: options_(options),
ceres_solver_options_(
common::CreateCeresSolverOptions(options.ceres_solver_options())) {
ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
}
CeresScanMatcher::~CeresScanMatcher() {}
CeresScanMatcher2D::~CeresScanMatcher2D() {}
void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const mapping::ProbabilityGrid& probability_grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const {
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const {
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle()};
ceres::Problem problem;
CHECK_GT(options_.occupied_space_weight(), 0.);
problem.AddResidualBlock(
OccupiedSpaceCostFunction::CreateAutoDiffCostFunction(
OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, probability_grid),
nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock(
TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(
RotationDeltaCostFunctor::CreateAutoDiffCostFunction(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.rotation_weight(), ceres_pose_estimate[2]),
nullptr /* loss function */, ceres_pose_estimate);
@ -94,5 +94,5 @@ void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
}
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

28
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h → cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_
#include <memory>
#include <vector>
@ -23,25 +23,25 @@
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.pb.h"
#include "cartographer/sensor/point_cloud.h"
#include "ceres/ceres.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D(
common::LuaParameterDictionary* parameter_dictionary);
// Align scans with an existing map using Ceres.
class CeresScanMatcher {
class CeresScanMatcher2D {
public:
explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options);
virtual ~CeresScanMatcher();
explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options);
virtual ~CeresScanMatcher2D();
CeresScanMatcher(const CeresScanMatcher&) = delete;
CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
CeresScanMatcher2D(const CeresScanMatcher2D&) = delete;
CeresScanMatcher2D& operator=(const CeresScanMatcher2D&) = delete;
// Aligns 'point_cloud' within the 'probability_grid' given an
// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
@ -49,17 +49,17 @@ class CeresScanMatcher {
void Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const mapping::ProbabilityGrid& probability_grid,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate,
ceres::Solver::Summary* summary) const;
private:
const proto::CeresScanMatcherOptions options_;
const proto::CeresScanMatcherOptions2D options_;
ceres::Solver::Options ceres_solver_options_;
};
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_

22
cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc → cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
#include <memory>
@ -28,18 +28,18 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
namespace {
class CeresScanMatcherTest : public ::testing::Test {
protected:
CeresScanMatcherTest()
: probability_grid_(mapping::MapLimits(1., Eigen::Vector2d(10., 10.),
mapping::CellLimits(20, 20))) {
: probability_grid_(
MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) {
probability_grid_.SetProbability(
probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
mapping::kMaxProbability);
kMaxProbability);
point_cloud_.emplace_back(-3.f, 2.f, 0.f);
@ -54,9 +54,9 @@ class CeresScanMatcherTest : public ::testing::Test {
num_threads = 1,
},
})text");
const proto::CeresScanMatcherOptions options =
CreateCeresScanMatcherOptions(parameter_dictionary.get());
ceres_scan_matcher_ = common::make_unique<CeresScanMatcher>(options);
const proto::CeresScanMatcherOptions2D options =
CreateCeresScanMatcherOptions2D(parameter_dictionary.get());
ceres_scan_matcher_ = common::make_unique<CeresScanMatcher2D>(options);
}
void TestFromInitialPose(const transform::Rigid2d& initial_pose) {
@ -73,9 +73,9 @@ class CeresScanMatcherTest : public ::testing::Test {
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
}
mapping::ProbabilityGrid probability_grid_;
ProbabilityGrid probability_grid_;
sensor::PointCloud point_cloud_;
std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_;
std::unique_ptr<CeresScanMatcher2D> ceres_scan_matcher_;
};
TEST_F(CeresScanMatcherTest, testPerfectEstimate) {
@ -96,5 +96,5 @@ TEST_F(CeresScanMatcherTest, testOptimizeAlongXY) {
} // namespace
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

11
cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc → cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc
View File

@ -14,14 +14,14 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h"
#include <cmath>
#include "cartographer/common/math.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
SearchParameters::SearchParameters(const double linear_search_window,
@ -71,7 +71,7 @@ SearchParameters::SearchParameters(const int num_linear_perturbations,
}
void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan>& scans,
const mapping::CellLimits& cell_limits) {
const CellLimits& cell_limits) {
CHECK_EQ(scans.size(), num_scans);
CHECK_EQ(linear_bounds.size(), num_scans);
for (int i = 0; i != num_scans; ++i) {
@ -109,8 +109,7 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
}
std::vector<DiscreteScan> DiscretizeScans(
const mapping::MapLimits& map_limits,
const std::vector<sensor::PointCloud>& scans,
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan> discrete_scans;
discrete_scans.reserve(scans.size());
@ -128,5 +127,5 @@ std::vector<DiscreteScan> DiscretizeScans(
}
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

15
cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h → cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
#include <vector>
@ -26,7 +26,7 @@
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
typedef std::vector<Eigen::Array2i> DiscreteScan;
@ -50,7 +50,7 @@ struct SearchParameters {
// Tightens the search window as much as possible.
void ShrinkToFit(const std::vector<DiscreteScan>& scans,
const mapping::CellLimits& cell_limits);
const CellLimits& cell_limits);
int num_angular_perturbations;
double angular_perturbation_step_size;
@ -67,8 +67,7 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
// Translates and discretizes the rotated scans into a vector of integer
// indices.
std::vector<DiscreteScan> DiscretizeScans(
const mapping::MapLimits& map_limits,
const std::vector<sensor::PointCloud>& scans,
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation);
// A possible solution.
@ -104,7 +103,7 @@ struct Candidate {
};
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_

10
cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc
View File

@ -14,13 +14,13 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h"
#include "cartographer/sensor/point_cloud.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
namespace {
@ -79,8 +79,8 @@ TEST(DiscretizeScans, DiscretizeScans) {
point_cloud.emplace_back(-0.125f, 0.125f, 0.f);
point_cloud.emplace_back(-0.125f, 0.075f, 0.f);
point_cloud.emplace_back(-0.125f, 0.025f, 0.f);
const mapping::MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
mapping::CellLimits(6, 6));
const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
CellLimits(6, 6));
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans =
@ -98,5 +98,5 @@ TEST(DiscretizeScans, DiscretizeScans) {
} // namespace
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

52
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc → cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
#include <algorithm>
#include <cmath>
@ -30,9 +30,8 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
namespace {
// A collection of values which can be added and later removed, and the maximum
@ -75,10 +74,10 @@ class SlidingWindowMaximum {
} // namespace
proto::FastCorrelativeScanMatcherOptions
CreateFastCorrelativeScanMatcherOptions(
proto::FastCorrelativeScanMatcherOptions2D
CreateFastCorrelativeScanMatcherOptions2D(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::FastCorrelativeScanMatcherOptions options;
proto::FastCorrelativeScanMatcherOptions2D options;
options.set_linear_search_window(
parameter_dictionary->GetDouble("linear_search_window"));
options.set_angular_search_window(
@ -89,9 +88,8 @@ CreateFastCorrelativeScanMatcherOptions(
}
PrecomputationGrid::PrecomputationGrid(
const mapping::ProbabilityGrid& probability_grid,
const mapping::CellLimits& limits, const int width,
std::vector<float>* reusable_intermediate_grid)
const ProbabilityGrid& probability_grid, const CellLimits& limits,
const int width, std::vector<float>* reusable_intermediate_grid)
: offset_(-width + 1, -width + 1),
wide_limits_(limits.num_x_cells + width - 1,
limits.num_y_cells + width - 1),
@ -160,9 +158,9 @@ PrecomputationGrid::PrecomputationGrid(
}
uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
const int cell_value = common::RoundToInt(
(probability - mapping::kMinProbability) *
(255.f / (mapping::kMaxProbability - mapping::kMinProbability)));
const int cell_value =
common::RoundToInt((probability - kMinProbability) *
(255.f / (kMaxProbability - kMinProbability)));
CHECK_GE(cell_value, 0);
CHECK_LE(cell_value, 255);
return cell_value;
@ -171,13 +169,13 @@ uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
class PrecomputationGridStack {
public:
PrecomputationGridStack(
const mapping::ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions& options) {
const ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions2D& options) {
CHECK_GE(options.branch_and_bound_depth(), 1);
const int max_width = 1 << (options.branch_and_bound_depth() - 1);
precomputation_grids_.reserve(options.branch_and_bound_depth());
std::vector<float> reusable_intermediate_grid;
const mapping::CellLimits limits = probability_grid.limits().cell_limits();
const CellLimits limits = probability_grid.limits().cell_limits();
reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
limits.num_y_cells);
for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
@ -197,17 +195,17 @@ class PrecomputationGridStack {
std::vector<PrecomputationGrid> precomputation_grids_;
};
FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const mapping::ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions& options)
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
const ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions2D& options)
: options_(options),
limits_(probability_grid.limits()),
precomputation_grid_stack_(
new PrecomputationGridStack(probability_grid, options)) {}
FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {}
bool FastCorrelativeScanMatcher::Match(
bool FastCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
@ -219,7 +217,7 @@ bool FastCorrelativeScanMatcher::Match(
pose_estimate);
}
bool FastCorrelativeScanMatcher::MatchFullSubmap(
bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
// Compute a search window around the center of the submap that includes it
@ -236,7 +234,7 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
min_score, score, pose_estimate);
}
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, float min_score, float* score,
@ -274,7 +272,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
}
std::vector<Candidate>
FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters) const {
std::vector<Candidate> lowest_resolution_candidates =
@ -286,7 +284,7 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
}
std::vector<Candidate>
FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters) const {
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
int num_candidates = 0;
@ -323,7 +321,7 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
return candidates;
}
void FastCorrelativeScanMatcher::ScoreCandidates(
void FastCorrelativeScanMatcher2D::ScoreCandidates(
const PrecomputationGrid& precomputation_grid,
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
@ -343,7 +341,7 @@ void FastCorrelativeScanMatcher::ScoreCandidates(
std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
}
Candidate FastCorrelativeScanMatcher::BranchAndBound(
Candidate FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate>& candidates, const int candidate_depth,
@ -389,5 +387,5 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
}
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

47
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h → cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h
View File

@ -22,8 +22,8 @@
// precomputation done for a given map. However, this map is immutable after
// construction.
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_
#include <memory>
#include <vector>
@ -32,16 +32,16 @@
#include "cartographer/common/port.h"
#include "cartographer/mapping/probability_values.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h"
#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.pb.h"
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
proto::FastCorrelativeScanMatcherOptions
CreateFastCorrelativeScanMatcherOptions(
proto::FastCorrelativeScanMatcherOptions2D
CreateFastCorrelativeScanMatcherOptions2D(
common::LuaParameterDictionary* parameter_dictionary);
// A precomputed grid that contains in each cell (x0, y0) the maximum
@ -49,8 +49,8 @@ CreateFastCorrelativeScanMatcherOptions(
// y0 <= y < y0.
class PrecomputationGrid {
public:
PrecomputationGrid(const mapping::ProbabilityGrid& probability_grid,
const mapping::CellLimits& limits, int width,
PrecomputationGrid(const ProbabilityGrid& probability_grid,
const CellLimits& limits, int width,
std::vector<float>* reusable_intermediate_grid);
// Returns a value between 0 and 255 to represent probabilities between
@ -74,9 +74,8 @@ class PrecomputationGrid {
// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
static float ToProbability(float value) {
return mapping::kMinProbability +
value *
((mapping::kMaxProbability - mapping::kMinProbability) / 255.f);
return kMinProbability +
value * ((kMaxProbability - kMinProbability) / 255.f);
}
private:
@ -87,7 +86,7 @@ class PrecomputationGrid {
const Eigen::Array2i offset_;
// Size of the precomputation grid.
const mapping::CellLimits wide_limits_;
const CellLimits wide_limits_;
// Probabilites mapped to 0 to 255.
std::vector<uint8> cells_;
@ -96,15 +95,15 @@ class PrecomputationGrid {
class PrecomputationGridStack;
// An implementation of "Real-Time Correlative Scan Matching" by Olson.
class FastCorrelativeScanMatcher {
class FastCorrelativeScanMatcher2D {
public:
FastCorrelativeScanMatcher(
const mapping::ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions& options);
~FastCorrelativeScanMatcher();
FastCorrelativeScanMatcher2D(
const ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions2D& options);
~FastCorrelativeScanMatcher2D();
FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete;
FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) =
FastCorrelativeScanMatcher2D(const FastCorrelativeScanMatcher2D&) = delete;
FastCorrelativeScanMatcher2D& operator=(const FastCorrelativeScanMatcher2D&) =
delete;
// Aligns 'point_cloud' within the 'probability_grid' given an
@ -145,13 +144,13 @@ class FastCorrelativeScanMatcher {
const std::vector<Candidate>& candidates,
int candidate_depth, float min_score) const;
const proto::FastCorrelativeScanMatcherOptions options_;
mapping::MapLimits limits_;
const proto::FastCorrelativeScanMatcherOptions2D options_;
MapLimits limits_;
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
};
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_

59
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc → cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
#include <algorithm>
#include <cmath>
@ -30,7 +30,7 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
namespace {
@ -39,10 +39,10 @@ TEST(PrecomputationGridTest, CorrectValues) {
// represented by uint8 values.
std::mt19937 prng(42);
std::uniform_int_distribution<int> distribution(0, 255);
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(250, 250)));
for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator(
Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
probability_grid.SetProbability(
xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
}
@ -52,8 +52,8 @@ TEST(PrecomputationGridTest, CorrectValues) {
PrecomputationGrid precomputation_grid(
probability_grid, probability_grid.limits().cell_limits(), width,
&reusable_intermediate_grid);
for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator(
probability_grid.limits().cell_limits())) {
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
float max_score = -std::numeric_limits<float>::infinity();
for (int y = 0; y != width; ++y) {
for (int x = 0; x != width; ++x) {
@ -73,10 +73,10 @@ TEST(PrecomputationGridTest, CorrectValues) {
TEST(PrecomputationGridTest, TinyProbabilityGrid) {
std::mt19937 prng(42);
std::uniform_int_distribution<int> distribution(0, 255);
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
0.05, Eigen::Vector2d(0.1, 0.1), mapping::CellLimits(4, 4)));
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
for (const Eigen::Array2i& xy_index :
mapping::XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
probability_grid.SetProbability(
xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
}
@ -86,8 +86,8 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
PrecomputationGrid precomputation_grid(
probability_grid, probability_grid.limits().cell_limits(), width,
&reusable_intermediate_grid);
for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator(
probability_grid.limits().cell_limits())) {
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
float max_score = -std::numeric_limits<float>::infinity();
for (int y = 0; y != width; ++y) {
for (int x = 0; x != width; ++x) {
@ -104,8 +104,9 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
}
}
proto::FastCorrelativeScanMatcherOptions
CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
proto::FastCorrelativeScanMatcherOptions2D
CreateFastCorrelativeScanMatcherTestOptions2D(
const int branch_and_bound_depth) {
auto parameter_dictionary =
common::MakeDictionary(R"text(
return {
@ -113,7 +114,7 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
angular_search_window = 1.,
branch_and_bound_depth = )text" +
std::to_string(branch_and_bound_depth) + "}");
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
return CreateFastCorrelativeScanMatcherOptions2D(parameter_dictionary.get());
}
mapping::proto::RangeDataInserterOptions2D
@ -130,10 +131,10 @@ CreateRangeDataInserterTestOptions2D() {
TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
std::mt19937 prng(42);
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
mapping::RangeDataInserter2D range_data_inserter(
RangeDataInserter2D range_data_inserter(
CreateRangeDataInserterTestOptions2D());
constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(3);
const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3);
sensor::PointCloud point_cloud;
point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
@ -148,8 +149,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
{2. * distribution(prng), 2. * distribution(prng)},
0.5 * distribution(prng));
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200)));
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
range_data_inserter.Insert(
sensor::RangeData{
Eigen::Vector3f(expected_pose.translation().x(),
@ -160,8 +161,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
&probability_grid);
probability_grid.FinishUpdate();
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
options);
FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid,
options);
transform::Rigid2d pose_estimate;
float score;
EXPECT_TRUE(fast_correlative_scan_matcher.Match(
@ -178,10 +179,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
std::mt19937 prng(42);
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
mapping::RangeDataInserter2D range_data_inserter(
RangeDataInserter2D range_data_inserter(
CreateRangeDataInserterTestOptions2D());
constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(6);
const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6);
sensor::PointCloud unperturbed_point_cloud;
unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
@ -202,8 +203,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
0.5 * distribution(prng)) *
perturbation.inverse();
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200)));
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
range_data_inserter.Insert(
sensor::RangeData{
transform::Embed3D(expected_pose * perturbation).translation(),
@ -213,8 +214,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
&probability_grid);
probability_grid.FinishUpdate();
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
options);
FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid,
options);
transform::Rigid2d pose_estimate;
float score;
EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap(
@ -229,5 +230,5 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
} // namespace
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

13
cartographer/mapping_2d/scan_matching/fast_global_localizer.cc
View File

@ -19,16 +19,13 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
bool PerformGlobalLocalization(
const float cutoff,
const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter,
const std::vector<
cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>&
matchers,
const cartographer::sensor::PointCloud& point_cloud,
const float cutoff, const sensor::AdaptiveVoxelFilter& voxel_filter,
const std::vector<FastCorrelativeScanMatcher2D*>& 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!";
@ -57,5 +54,5 @@ bool PerformGlobalLocalization(
}
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

14
cartographer/mapping_2d/scan_matching/fast_global_localizer.h
View File

@ -20,11 +20,11 @@
#include <vector>
#include "Eigen/Geometry"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
#include "cartographer/sensor/voxel_filter.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
// Perform global localization against the provided 'matchers'. The 'cutoff'
@ -37,15 +37,13 @@ namespace scan_matching {
// 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 cartographer::sensor::AdaptiveVoxelFilter& voxel_filter,
const std::vector<
cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>&
matchers,
const cartographer::sensor::PointCloud& point_cloud,
float cutoff, const sensor::AdaptiveVoxelFilter& voxel_filter,
const std::vector<scan_matching::FastCorrelativeScanMatcher2D*>& matchers,
const sensor::PointCloud& point_cloud,
transform::Rigid2d* best_pose_estimate, float* best_score);
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_

4
cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto → cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto
View File

@ -14,12 +14,12 @@
syntax = "proto3";
package cartographer.mapping_2d.scan_matching.proto;
package cartographer.mapping.scan_matching.proto;
import "cartographer/common/proto/ceres_solver_options.proto";
// NEXT ID: 10
message CeresScanMatcherOptions {
message CeresScanMatcherOptions2D {
// Scaling parameters for each cost functor.
double occupied_space_weight = 1;
double translation_weight = 2;

4
cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto → cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto
View File

@ -14,9 +14,9 @@
syntax = "proto3";
package cartographer.mapping_2d.scan_matching.proto;
package cartographer.mapping.scan_matching.proto;
message FastCorrelativeScanMatcherOptions {
message FastCorrelativeScanMatcherOptions2D {
// Minimum linear search window in which the best possible scan alignment
// will be found.
double linear_search_window = 3;

35
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc → cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
#include <algorithm>
#include <cmath>
@ -30,32 +30,15 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
proto::RealTimeCorrelativeScanMatcherOptions
CreateRealTimeCorrelativeScanMatcherOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::RealTimeCorrelativeScanMatcherOptions options;
options.set_linear_search_window(
parameter_dictionary->GetDouble("linear_search_window"));
options.set_angular_search_window(
parameter_dictionary->GetDouble("angular_search_window"));
options.set_translation_delta_cost_weight(
parameter_dictionary->GetDouble("translation_delta_cost_weight"));
options.set_rotation_delta_cost_weight(
parameter_dictionary->GetDouble("rotation_delta_cost_weight"));
CHECK_GE(options.translation_delta_cost_weight(), 0.);
CHECK_GE(options.rotation_delta_cost_weight(), 0.);
return options;
}
RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D(
const proto::RealTimeCorrelativeScanMatcherOptions& options)
: options_(options) {}
std::vector<Candidate>
RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
const SearchParameters& search_parameters) const {
int num_candidates = 0;
for (int scan_index = 0; scan_index != search_parameters.num_scans;
@ -88,10 +71,10 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
return candidates;
}
double RealTimeCorrelativeScanMatcher::Match(
double RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const mapping::ProbabilityGrid& probability_grid,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate);
@ -124,8 +107,8 @@ double RealTimeCorrelativeScanMatcher::Match(
return best_candidate.score;
}
void RealTimeCorrelativeScanMatcher::ScoreCandidates(
const mapping::ProbabilityGrid& probability_grid,
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
const ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* const candidates) const {
@ -152,5 +135,5 @@ void RealTimeCorrelativeScanMatcher::ScoreCandidates(
}
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

33
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h → cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h
View File

@ -33,43 +33,40 @@
// This can be made even faster by transforming the scan exactly once over some
// discretized range.
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_
#include <iostream>
#include <memory>
#include <vector>
#include "Eigen/Core"
#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
proto::RealTimeCorrelativeScanMatcherOptions
CreateRealTimeCorrelativeScanMatcherOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// An implementation of "Real-Time Correlative Scan Matching" by Olson.
class RealTimeCorrelativeScanMatcher {
class RealTimeCorrelativeScanMatcher2D {
public:
explicit RealTimeCorrelativeScanMatcher(
explicit RealTimeCorrelativeScanMatcher2D(
const proto::RealTimeCorrelativeScanMatcherOptions& options);
RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) =
RealTimeCorrelativeScanMatcher2D(const RealTimeCorrelativeScanMatcher2D&) =
delete;
RealTimeCorrelativeScanMatcher& operator=(
const RealTimeCorrelativeScanMatcher&) = delete;
RealTimeCorrelativeScanMatcher2D& operator=(
const RealTimeCorrelativeScanMatcher2D&) = delete;
// Aligns 'point_cloud' within the 'probability_grid' given an
// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
// returns the score.
double Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const mapping::ProbabilityGrid& probability_grid,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const;
// Computes the score for each Candidate in a collection. The cost is computed
@ -77,7 +74,7 @@ class RealTimeCorrelativeScanMatcher {
// http://ceres-solver.org/modeling.html
//
// Visible for testing.
void ScoreCandidates(const mapping::ProbabilityGrid& probability_grid,
void ScoreCandidates(const ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* candidates) const;
@ -90,7 +87,7 @@ class RealTimeCorrelativeScanMatcher {
};
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_

23
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc → cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
#include <cmath>
#include <memory>
@ -29,15 +29,15 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
namespace {
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
protected:
RealTimeCorrelativeScanMatcherTest()
: probability_grid_(mapping::MapLimits(0.05, Eigen::Vector2d(0.05, 0.25),
mapping::CellLimits(6, 6))) {
: probability_grid_(
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
{
auto parameter_dictionary = common::MakeDictionary(
"return { "
@ -45,9 +45,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
"hit_probability = 0.7, "
"miss_probability = 0.4, "
"}");
range_data_inserter_ = common::make_unique<mapping::RangeDataInserter2D>(
mapping::CreateRangeDataInserterOptions2D(
parameter_dictionary.get()));
range_data_inserter_ = common::make_unique<RangeDataInserter2D>(
CreateRangeDataInserterOptions2D(parameter_dictionary.get()));
}
point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
@ -69,16 +68,16 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
"rotation_delta_cost_weight = 0., "
"}");
real_time_correlative_scan_matcher_ =
common::make_unique<RealTimeCorrelativeScanMatcher>(
common::make_unique<RealTimeCorrelativeScanMatcher2D>(
CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary.get()));
}
}
mapping::ProbabilityGrid probability_grid_;
std::unique_ptr<mapping::RangeDataInserter2D> range_data_inserter_;
ProbabilityGrid probability_grid_;
std::unique_ptr<RangeDataInserter2D> range_data_inserter_;
sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher>
std::unique_ptr<RealTimeCorrelativeScanMatcher2D>
real_time_correlative_scan_matcher_;
};
@ -121,5 +120,5 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
} // namespace
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

25
cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h → cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor_2d.h
View File

@ -14,24 +14,24 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_
#include "Eigen/Core"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with
// the solution's distance from 'target_angle'.
class RotationDeltaCostFunctor {
class RotationDeltaCostFunctor2D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const double target_angle) {
return new ceres::AutoDiffCostFunction<
RotationDeltaCostFunctor, 1 /* residuals */, 3 /* pose variables */>(
new RotationDeltaCostFunctor(scaling_factor, target_angle));
RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>(
new RotationDeltaCostFunctor2D(scaling_factor, target_angle));
}
template <typename T>
@ -41,19 +41,20 @@ class RotationDeltaCostFunctor {
}
private:
explicit RotationDeltaCostFunctor(const double scaling_factor,
const double target_angle)
explicit RotationDeltaCostFunctor2D(const double scaling_factor,
const double target_angle)
: scaling_factor_(scaling_factor), angle_(target_angle) {}
RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor2D(const RotationDeltaCostFunctor2D&) = delete;
RotationDeltaCostFunctor2D& operator=(const RotationDeltaCostFunctor2D&) =
delete;
const double scaling_factor_;
const double angle_;
};
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_

29
cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h → cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h
View File

@ -14,24 +14,25 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_
#include "Eigen/Core"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace scan_matching {
// Computes the cost of translating 'pose' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor {
class TranslationDeltaCostFunctor2D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Vector2d& target_translation) {
return new ceres::AutoDiffCostFunction<
TranslationDeltaCostFunctor, 2 /* residuals */, 3 /* pose variables */>(
new TranslationDeltaCostFunctor(scaling_factor, target_translation));
return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor2D,
2 /* residuals */,
3 /* pose variables */>(
new TranslationDeltaCostFunctor2D(scaling_factor, target_translation));
}
template <typename T>
@ -42,17 +43,17 @@ class TranslationDeltaCostFunctor {
}
private:
// Constructs a new TranslationDeltaCostFunctor from the given
// Constructs a new TranslationDeltaCostFunctor2D from the given
// 'target_translation' (x, y).
explicit TranslationDeltaCostFunctor(
explicit TranslationDeltaCostFunctor2D(
const double scaling_factor, const Eigen::Vector2d& target_translation)
: scaling_factor_(scaling_factor),
x_(target_translation.x()),
y_(target_translation.y()) {}
TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete;
TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =
delete;
TranslationDeltaCostFunctor2D(const TranslationDeltaCostFunctor2D&) = delete;
TranslationDeltaCostFunctor2D& operator=(
const TranslationDeltaCostFunctor2D&) = delete;
const double scaling_factor_;
const double x_;
@ -60,7 +61,7 @@ class TranslationDeltaCostFunctor {
};
} // namespace scan_matching
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_

4
cartographer/mapping_3d/local_trajectory_builder_options_3d.cc
View File

@ -17,7 +17,7 @@
#include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h"
#include "cartographer/internal/mapping/motion_filter.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/voxel_filter.h"
@ -48,7 +48,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
options.set_use_online_correlative_scan_matching(
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
*options.mutable_real_time_correlative_scan_matcher_options() =
mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary
->GetDictionary("real_time_correlative_scan_matcher")
.get());

4
cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto
View File

@ -18,7 +18,7 @@ package cartographer.mapping.proto;
import "cartographer/mapping/proto/motion_filter_options.proto";
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
import "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
import "cartographer/mapping_3d/proto/submaps_options_3d.proto";
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
@ -45,7 +45,7 @@ message LocalTrajectoryBuilderOptions3D {
// Whether to solve the online scan matching first using the correlative scan
// matcher to generate a good starting point for Ceres.
bool use_online_correlative_scan_matching = 13;
mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 14;
mapping_3d.scan_matching.proto.CeresScanMatcherOptions
ceres_scan_matcher_options = 6;

4
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h
View File

@ -15,7 +15,7 @@
*/
// This is an implementation of a 3D branch-and-bound algorithm similar to
// mapping_2d::FastCorrelativeScanMatcher.
// FastCorrelativeScanMatcher2D.
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
@ -26,7 +26,7 @@
#include "Eigen/Core"
#include "cartographer/common/port.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"

4
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc
View File

@ -28,8 +28,8 @@ namespace mapping_3d {
namespace scan_matching {
RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
const mapping_2d::scan_matching::proto::
RealTimeCorrelativeScanMatcherOptions& options)
const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions&
options)
: options_(options) {}
float RealTimeCorrelativeScanMatcher::Match(

6
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h
View File

@ -22,7 +22,7 @@
#include <vector>
#include "Eigen/Core"
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/sensor/point_cloud.h"
@ -33,7 +33,7 @@ namespace scan_matching {
class RealTimeCorrelativeScanMatcher {
public:
explicit RealTimeCorrelativeScanMatcher(
const mapping_2d::scan_matching::proto::
const mapping::scan_matching::proto::
RealTimeCorrelativeScanMatcherOptions& options);
RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) =
@ -56,7 +56,7 @@ class RealTimeCorrelativeScanMatcher {
const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const;
const mapping_2d::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions
const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions
options_;
};

7
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -20,7 +20,7 @@
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
@ -58,9 +58,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
})text");
real_time_correlative_scan_matcher_.reset(
new RealTimeCorrelativeScanMatcher(
mapping_2d::scan_matching::
CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary.get())));
mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary.get())));
}
void TestFromInitialPose(const transform::Rigid3d& initial_pose) {

4
cartographer/metrics/register.cc
View File

@ -16,14 +16,14 @@
#include "cartographer/metrics/register.h"
#include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h"
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
namespace cartographer {
namespace metrics {
void RegisterAllMetrics(FamilyFactory* registry) {
mapping_2d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry);
mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
}