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

Remove 'mapping_2d' namespace. (#922) 

It is removed from everywhere but 'scan_matching', 'pose_graph'
subfolders of 'mapping_2d'.

[Code structure RFC](e11bca586f/text/0000-code-structure.md)
master
Alexander Belyaev 2018-02-21 12:41:14 +01:00 committed by GitHub
parent 96d5e2819c
commit f8dc89d8ff
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
55 changed files with 337 additions and 342 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cartographer/internal/mapping_2d/local_trajectory_builder.cc
View File

@ -26,7 +26,7 @@ namespace cartographer {
namespace mapping_2d {
LocalTrajectoryBuilder::LocalTrajectoryBuilder(
const proto::LocalTrajectoryBuilderOptions& options)
const mapping::proto::LocalTrajectoryBuilderOptions2D& options)
: options_(options),
active_submaps_(options.submaps_options()),
motion_filter_(options_.motion_filter_options()),
@ -53,7 +53,7 @@ LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter(
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
const common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::RangeData& gravity_aligned_range_data) {
std::shared_ptr<const Submap> matching_submap =
std::shared_ptr<const mapping::Submap2D> matching_submap =
active_submaps_.submaps().front();
// The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
@ -205,8 +205,9 @@ 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 Submap>> insertion_submaps;
for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps()) {
std::vector<std::shared_ptr<const mapping::Submap2D>> insertion_submaps;
for (const std::shared_ptr<mapping::Submap2D>& submap :
active_submaps_.submaps()) {
insertion_submaps.push_back(submap);
}
active_submaps_.InsertRangeData(range_data_in_local);

12
cartographer/internal/mapping_2d/local_trajectory_builder.h
View File

@ -22,10 +22,10 @@
#include "cartographer/common/time.h"
#include "cartographer/internal/mapping/motion_filter.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.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/submaps.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/range_data.h"
@ -42,7 +42,7 @@ class LocalTrajectoryBuilder {
public:
struct InsertionResult {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
std::vector<std::shared_ptr<const mapping::Submap2D>> insertion_submaps;
};
struct MatchingResult {
common::Time time;
@ -53,7 +53,7 @@ class LocalTrajectoryBuilder {
};
explicit LocalTrajectoryBuilder(
const proto::LocalTrajectoryBuilderOptions& options);
const mapping::proto::LocalTrajectoryBuilderOptions2D& options);
~LocalTrajectoryBuilder();
LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
@ -92,8 +92,8 @@ class LocalTrajectoryBuilder {
// Lazily constructs a PoseExtrapolator.
void InitializeExtrapolator(common::Time time);
const proto::LocalTrajectoryBuilderOptions options_;
ActiveSubmaps active_submaps_;
const mapping::proto::LocalTrajectoryBuilderOptions2D options_;
mapping::ActiveSubmaps2D active_submaps_;
mapping::MotionFilter motion_filter_;
scan_matching::RealTimeCorrelativeScanMatcher

12
cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h
View File

@ -36,7 +36,7 @@ class OccupiedSpaceCostFunction {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid) {
const mapping::ProbabilityGrid& probability_grid) {
return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
ceres::DYNAMIC /* residuals */,
3 /* pose variables */>(
@ -55,7 +55,7 @@ class OccupiedSpaceCostFunction {
const GridArrayAdapter adapter(probability_grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
const MapLimits& limits = probability_grid_.limits();
const mapping::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,7 +79,7 @@ class OccupiedSpaceCostFunction {
public:
enum { DATA_DIMENSION = 1 };
explicit GridArrayAdapter(const ProbabilityGrid& probability_grid)
explicit GridArrayAdapter(const mapping::ProbabilityGrid& probability_grid)
: probability_grid_(probability_grid) {}
void GetValue(const int row, const int column, double* const value) const {
@ -103,12 +103,12 @@ class OccupiedSpaceCostFunction {
}
private:
const ProbabilityGrid& probability_grid_;
const mapping::ProbabilityGrid& probability_grid_;
};
OccupiedSpaceCostFunction(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid)
const mapping::ProbabilityGrid& probability_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
probability_grid_(probability_grid) {}
@ -119,7 +119,7 @@ class OccupiedSpaceCostFunction {
const double scaling_factor_;
const sensor::PointCloud& point_cloud_;
const ProbabilityGrid& probability_grid_;
const mapping::ProbabilityGrid& probability_grid_;
};
} // namespace scan_matching

23
cartographer/io/probability_grid_points_processor.cc
View File

@ -30,7 +30,7 @@ namespace io {
namespace {
void DrawTrajectoriesIntoImage(
const mapping_2d::ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
const Eigen::Array2i& offset,
const std::vector<mapping::proto::Trajectory>& trajectories,
cairo_surface_t* cairo_surface) {
@ -57,7 +57,7 @@ uint8 ProbabilityToColor(float probability_from_grid) {
ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
const double resolution,
const mapping_2d::proto::RangeDataInserterOptions&
const mapping::proto::RangeDataInserterOptions2D&
range_data_inserter_options,
const DrawTrajectories& draw_trajectories,
std::unique_ptr<FileWriter> file_writer,
@ -82,7 +82,7 @@ ProbabilityGridPointsProcessor::FromDictionary(
: DrawTrajectories::kNo;
return common::make_unique<ProbabilityGridPointsProcessor>(
dictionary->GetDouble("resolution"),
mapping_2d::CreateRangeDataInserterOptions(
mapping::CreateRangeDataInserterOptions2D(
dictionary->GetDictionary("range_data_inserter").get()),
draw_trajectories,
file_writer_factory(dictionary->GetString("filename") + ".png"),
@ -125,9 +125,8 @@ PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
}
std::unique_ptr<Image> DrawProbabilityGrid(
const mapping_2d::ProbabilityGrid& probability_grid,
Eigen::Array2i* offset) {
mapping_2d::CellLimits cell_limits;
const mapping::ProbabilityGrid& probability_grid, Eigen::Array2i* offset) {
mapping::CellLimits cell_limits;
probability_grid.ComputeCroppedLimits(offset, &cell_limits);
if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
LOG(WARNING) << "Not writing output: empty probability grid";
@ -136,7 +135,7 @@ std::unique_ptr<Image> DrawProbabilityGrid(
auto image = common::make_unique<Image>(cell_limits.num_x_cells,
cell_limits.num_y_cells);
for (const Eigen::Array2i& xy_index :
cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
mapping::XYIndexRangeIterator(cell_limits)) {
const Eigen::Array2i index = xy_index + *offset;
constexpr uint8 kUnknownValue = 128;
const uint8 value =
@ -149,14 +148,14 @@ std::unique_ptr<Image> DrawProbabilityGrid(
return image;
}
mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) {
mapping::ProbabilityGrid CreateProbabilityGrid(const double resolution) {
constexpr int kInitialProbabilityGridSize = 100;
Eigen::Vector2d max =
0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones();
return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits(
resolution, max,
mapping_2d::CellLimits(kInitialProbabilityGridSize,
kInitialProbabilityGridSize)));
return mapping::ProbabilityGrid(
mapping::MapLimits(resolution, max,
mapping::CellLimits(kInitialProbabilityGridSize,
kInitialProbabilityGridSize)));
}
} // namespace io

15
cartographer/io/probability_grid_points_processor.h
View File

@ -25,8 +25,8 @@
#include "cartographer/io/points_processor.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_2d/proto/range_data_inserter_options_2d.pb.h"
#include "cartographer/mapping_2d/range_data_inserter_2d.h"
namespace cartographer {
namespace io {
@ -42,7 +42,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
enum class DrawTrajectories { kNo, kYes };
ProbabilityGridPointsProcessor(
double resolution,
const mapping_2d::proto::RangeDataInserterOptions&
const mapping::proto::RangeDataInserterOptions2D&
range_data_inserter_options,
const DrawTrajectories& draw_trajectories,
std::unique_ptr<FileWriter> file_writer,
@ -68,19 +68,18 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
const std::vector<mapping::proto::Trajectory> trajectories_;
std::unique_ptr<FileWriter> file_writer_;
PointsProcessor* const next_;
mapping_2d::RangeDataInserter range_data_inserter_;
mapping_2d::ProbabilityGrid probability_grid_;
mapping::RangeDataInserter2D range_data_inserter_;
mapping::ProbabilityGrid probability_grid_;
};
// Draws 'probability_grid' into an image and fills in 'offset' with the cropped
// map limits. Returns 'nullptr' if probability_grid was empty.
std::unique_ptr<Image> DrawProbabilityGrid(
const mapping_2d::ProbabilityGrid& probability_grid,
Eigen::Array2i* offset);
const mapping::ProbabilityGrid& probability_grid, Eigen::Array2i* offset);
// Create an initially empty probability grid with 'resolution' and a small
// size, suitable for a PointsBatchProcessor.
mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution);
mapping::ProbabilityGrid CreateProbabilityGrid(const double resolution);
} // namespace io
} // namespace cartographer

5
cartographer/io/submap_painter.cc
View File

@ -16,6 +16,9 @@
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/mapping_3d/submaps.h"
namespace cartographer {
namespace io {
namespace {
@ -115,7 +118,7 @@ void FillSubmapSlice(
local_pose = submap.local_pose();
submap.ToResponseProto(global_submap_pose, &response);
} else {
::cartographer::mapping_2d::Submap submap(proto.submap_2d());
::cartographer::mapping::Submap2D submap(proto.submap_2d());
local_pose = submap.local_pose();
submap.ToResponseProto(global_submap_pose, &response);
}

4
cartographer/io/submap_painter.h
View File

@ -21,9 +21,7 @@
#include "cairo/cairo.h"
#include "cartographer/io/image.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/proto/submap.pb.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/submaps.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {

2
cartographer/mapping/local_slam_result_data.cc
View File

@ -27,7 +27,7 @@ LocalSlamResultData::LocalSlamResultData(const std::string& sensor_id,
LocalSlamResult2D::LocalSlamResult2D(
const std::string& sensor_id, common::Time time,
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
const std::vector<std::shared_ptr<const mapping::Submap2D>>&
insertion_submaps)
: LocalSlamResultData(sensor_id, time),
node_data_(node_data),

5
cartographer/mapping/local_slam_result_data.h
View File

@ -42,8 +42,7 @@ class LocalSlamResult2D : public LocalSlamResultData {
LocalSlamResult2D(
const std::string& sensor_id, common::Time time,
std::shared_ptr<const TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
insertion_submaps);
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps);
void AddToTrajectoryBuilder(
TrajectoryBuilderInterface* const trajectory_builder) override;
@ -51,7 +50,7 @@ class LocalSlamResult2D : public LocalSlamResultData {
private:
std::shared_ptr<const TrajectoryNode::Data> node_data_;
std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps_;
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps_;
};
class LocalSlamResult3D : public LocalSlamResultData {

4
cartographer/mapping/proto/submap.proto
View File

@ -20,12 +20,12 @@ import "cartographer/mapping_2d/proto/probability_grid.proto";
import "cartographer/mapping_3d/proto/hybrid_grid.proto";
import "cartographer/transform/proto/transform.proto";
// Serialized state of a mapping_2d::Submap.
// Serialized state of a Submap2D.
message Submap2D {
transform.proto.Rigid3d local_pose = 1;
int32 num_range_data = 2;
bool finished = 3;
mapping_2d.proto.ProbabilityGrid probability_grid = 4;
mapping.proto.ProbabilityGrid probability_grid = 4;
}
// Serialized state of a mapping_3d::Submap.

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

@ -15,7 +15,7 @@
syntax = "proto3";
import "cartographer/transform/proto/transform.proto";
import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto";
import "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto";
import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto";
package cartographer.mapping.proto;
@ -27,8 +27,7 @@ message InitialTrajectoryPose {
}
message TrajectoryBuilderOptions {
mapping_2d.proto.LocalTrajectoryBuilderOptions
trajectory_builder_2d_options = 1;
LocalTrajectoryBuilderOptions2D trajectory_builder_2d_options = 1;
mapping_3d.proto.LocalTrajectoryBuilderOptions
trajectory_builder_3d_options = 2;
bool pure_localization = 3;

4
cartographer/mapping/trajectory_builder_interface.cc
View File

@ -17,7 +17,7 @@
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping_2d/local_trajectory_builder_options.h"
#include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
namespace cartographer {
@ -27,7 +27,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::TrajectoryBuilderOptions options;
*options.mutable_trajectory_builder_2d_options() =
mapping_2d::CreateLocalTrajectoryBuilderOptions(
CreateLocalTrajectoryBuilderOptions2D(
parameter_dictionary->GetDictionary("trajectory_builder_2d").get());
*options.mutable_trajectory_builder_3d_options() =
mapping_3d::CreateLocalTrajectoryBuilderOptions(

18
cartographer/mapping_2d/local_trajectory_builder_options.cc → cartographer/mapping_2d/local_trajectory_builder_options_2d.cc
View File

@ -14,20 +14,20 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/local_trajectory_builder_options.h"
#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/submaps.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/sensor/voxel_filter.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions options;
proto::LocalTrajectoryBuilderOptions2D options;
options.set_min_range(parameter_dictionary->GetDouble("min_range"));
options.set_max_range(parameter_dictionary->GetDouble("max_range"));
options.set_min_z(parameter_dictionary->GetDouble("min_z"));
@ -49,22 +49,22 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
->GetDictionary("loop_closure_adaptive_voxel_filter")
.get());
*options.mutable_real_time_correlative_scan_matcher_options() =
scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary
->GetDictionary("real_time_correlative_scan_matcher")
.get());
*options.mutable_ceres_scan_matcher_options() =
scan_matching::CreateCeresScanMatcherOptions(
mapping_2d::scan_matching::CreateCeresScanMatcherOptions(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions(
parameter_dictionary->GetDictionary("motion_filter").get());
options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
*options.mutable_submaps_options() = CreateSubmapsOptions(
*options.mutable_submaps_options() = CreateSubmapsOptions2D(
parameter_dictionary->GetDictionary("submaps").get());
options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data"));
return options;
}
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

14
cartographer/mapping_2d/local_trajectory_builder_options.h → cartographer/mapping_2d/local_trajectory_builder_options_2d.h
View File

@ -14,19 +14,19 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_
#define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.pb.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D(
common::LuaParameterDictionary* parameter_dictionary);
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_

4
cartographer/mapping_2d/map_limits.h
View File

@ -33,7 +33,7 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
// Defines the limits of a grid map. This class must remain inlined for
// performance reasons.
@ -97,7 +97,7 @@ inline proto::MapLimits ToProto(const MapLimits& map_limits) {
return result;
}
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_

4
cartographer/mapping_2d/map_limits_test.cc
View File

@ -19,7 +19,7 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace {
TEST(MapLimitsTest, ToProto) {
@ -63,5 +63,5 @@ TEST(MapLimitsTest, ConstructAndGet) {
}
} // namespace
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

14
cartographer/mapping_2d/pose_graph/constraint_builder.cc
View File

@ -38,7 +38,7 @@ namespace cartographer {
namespace mapping_2d {
namespace pose_graph {
transform::Rigid2d ComputeSubmapPose(const Submap& submap) {
transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap) {
return transform::Project2D(submap.local_pose());
}
@ -59,7 +59,7 @@ ConstraintBuilder::~ConstraintBuilder() {
}
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose) {
@ -84,7 +84,7 @@ void ConstraintBuilder::MaybeAddConstraint(
}
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data) {
common::MutexLocker locker(&mutex_);
@ -119,7 +119,8 @@ void ConstraintBuilder::WhenDone(
}
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap,
const mapping::SubmapId& submap_id,
const mapping::ProbabilityGrid* const submap,
const std::function<void()>& work_item) {
if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
nullptr) {
@ -134,7 +135,8 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
}
void ConstraintBuilder::ConstructSubmapScanMatcher(
const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap) {
const mapping::SubmapId& submap_id,
const mapping::ProbabilityGrid* const submap) {
auto submap_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
*submap, options_.fast_correlative_scan_matcher_options());
@ -157,7 +159,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
}
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
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,
const transform::Rigid2d& initial_relative_pose,

19
cartographer/mapping_2d/pose_graph/constraint_builder.h
View File

@ -34,9 +34,7 @@
#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/submaps.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/voxel_filter.h"
@ -46,7 +44,7 @@ namespace pose_graph {
// Returns (map <- submap) where 'submap' is a coordinate system at the origin
// of the Submap.
transform::Rigid2d ComputeSubmapPose(const Submap& submap);
transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap);
// Asynchronously computes constraints.
//
@ -76,7 +74,7 @@ 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 Submap* submap,
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);
@ -88,7 +86,7 @@ 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 Submap* submap,
const mapping::SubmapId& submap_id, const mapping::Submap2D* submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data);
@ -107,7 +105,7 @@ class ConstraintBuilder {
private:
struct SubmapScanMatcher {
const ProbabilityGrid* probability_grid;
const mapping::ProbabilityGrid* probability_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
fast_correlative_scan_matcher;
};
@ -115,12 +113,13 @@ class ConstraintBuilder {
// 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 ProbabilityGrid* submap,
const mapping::SubmapId& submap_id,
const mapping::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 ProbabilityGrid* submap)
const mapping::ProbabilityGrid* submap)
EXCLUDES(mutex_);
// Returns the scan matcher for a submap, which has to exist.
@ -131,7 +130,7 @@ class ConstraintBuilder {
// 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 Submap* submap,
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,

12
cartographer/mapping_2d/pose_graph_2d.cc
View File

@ -53,8 +53,7 @@ PoseGraph2D::~PoseGraph2D() {
std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
insertion_submaps) {
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) {
@ -103,8 +102,7 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
NodeId PoseGraph2D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
insertion_submaps) {
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
@ -234,7 +232,7 @@ void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
void PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
const bool newly_finished_submap) {
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
@ -431,8 +429,8 @@ void PoseGraph2D::AddSubmapFromProto(
const SubmapId submap_id = {submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
std::shared_ptr<const mapping_2d::Submap> submap_ptr =
std::make_shared<const mapping_2d::Submap>(submap.submap_2d());
std::shared_ptr<const Submap2D> submap_ptr =
std::make_shared<const Submap2D>(submap.submap_2d());
const transform::Rigid2d global_submap_pose_2d =
transform::Project2D(global_submap_pose);

19
cartographer/mapping_2d/pose_graph_2d.h
View File

@ -37,7 +37,7 @@
#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/submaps.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h"
@ -71,10 +71,11 @@ class PoseGraph2D : public PoseGraph {
// node data was inserted into the 'insertion_submaps'. If
// 'insertion_submaps.front().finished()' is 'true', data was inserted into
// this submap for the last time.
NodeId AddNode(std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
insertion_submaps) EXCLUDES(mutex_);
NodeId AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
EXCLUDES(mutex_);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_);
@ -137,7 +138,7 @@ class PoseGraph2D : public PoseGraph {
// Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished };
struct SubmapData {
std::shared_ptr<const mapping_2d::Submap> submap;
std::shared_ptr<const Submap2D> submap;
// IDs of the nodes that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
@ -157,13 +158,13 @@ class PoseGraph2D : public PoseGraph {
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
insertion_submaps) REQUIRES(mutex_);
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
REQUIRES(mutex_);
// Adds constraints for a node, and starts scan matching in the background.
void ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
bool newly_finished_submap) REQUIRES(mutex_);
// Computes constraints for a node and submap pair.

12
cartographer/mapping_2d/pose_graph_2d_test.cc
View File

@ -24,8 +24,8 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_2d/range_data_inserter_2d.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/rigid_transform_test_helpers.h"
#include "cartographer/transform/transform.h"
@ -58,8 +58,8 @@ class PoseGraph2DTest : public ::testing::Test {
miss_probability = 0.495,
},
})text");
active_submaps_ = common::make_unique<mapping_2d::ActiveSubmaps>(
mapping_2d::CreateSubmapsOptions(parameter_dictionary.get()));
active_submaps_ = common::make_unique<ActiveSubmaps2D>(
mapping::CreateSubmapsOptions2D(parameter_dictionary.get()));
}
{
@ -145,7 +145,7 @@ class PoseGraph2DTest : public ::testing::Test {
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
point_cloud_,
transform::Embed3D(current_pose_.inverse().cast<float>()));
std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps;
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
for (const auto& submap : active_submaps_->submaps()) {
insertion_submaps.push_back(submap);
}
@ -178,7 +178,7 @@ class PoseGraph2DTest : public ::testing::Test {
}
sensor::PointCloud point_cloud_;
std::unique_ptr<mapping_2d::ActiveSubmaps> active_submaps_;
std::unique_ptr<ActiveSubmaps2D> active_submaps_;
common::ThreadPool thread_pool_;
std::unique_ptr<PoseGraph2D> pose_graph_;
transform::Rigid2d current_pose_;

32
cartographer/mapping_2d/probability_grid.cc
View File

@ -5,7 +5,7 @@
#include "cartographer/mapping/probability_values.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace {
// Converts a 'cell_index' into an index into 'cells_'.
@ -20,7 +20,7 @@ ProbabilityGrid::ProbabilityGrid(const MapLimits& limits)
: limits_(limits),
cells_(
limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
mapping::kUnknownProbabilityValue) {}
kUnknownProbabilityValue) {}
ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto)
: limits_(proto.limits()), cells_() {
@ -40,8 +40,8 @@ ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto)
// Finishes the update sequence.
void ProbabilityGrid::FinishUpdate() {
while (!update_indices_.empty()) {
DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
cells_[update_indices_.back()] -= mapping::kUpdateMarker;
DCHECK_GE(cells_[update_indices_.back()], kUpdateMarker);
cells_[update_indices_.back()] -= kUpdateMarker;
update_indices_.pop_back();
}
}
@ -51,8 +51,8 @@ void ProbabilityGrid::FinishUpdate() {
void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
const float probability) {
uint16& cell = cells_[ToFlatIndex(cell_index, limits_)];
CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
cell = mapping::ProbabilityToValue(probability);
CHECK_EQ(cell, kUnknownProbabilityValue);
cell = ProbabilityToValue(probability);
known_cells_box_.extend(cell_index.matrix());
}
@ -65,15 +65,15 @@ void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
// will be set to probability corresponding to 'odds'.
bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
const std::vector<uint16>& table) {
DCHECK_EQ(table.size(), mapping::kUpdateMarker);
DCHECK_EQ(table.size(), kUpdateMarker);
const int flat_index = ToFlatIndex(cell_index, limits_);
uint16* cell = &cells_[flat_index];
if (*cell >= mapping::kUpdateMarker) {
if (*cell >= kUpdateMarker) {
return false;
}
update_indices_.push_back(flat_index);
*cell = table[*cell];
DCHECK_GE(*cell, mapping::kUpdateMarker);
DCHECK_GE(*cell, kUpdateMarker);
known_cells_box_.extend(cell_index.matrix());
return true;
}
@ -81,17 +81,15 @@ bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
// Returns the probability of the cell with 'cell_index'.
float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
if (limits_.Contains(cell_index)) {
return mapping::ValueToProbability(
cells_[ToFlatIndex(cell_index, limits_)]);
return ValueToProbability(cells_[ToFlatIndex(cell_index, limits_)]);
}
return mapping::kMinProbability;
return kMinProbability;
}
// Returns true if the probability at the specified index is known.
bool ProbabilityGrid::IsKnown(const Eigen::Array2i& cell_index) const {
return limits_.Contains(cell_index) &&
cells_[ToFlatIndex(cell_index, limits_)] !=
mapping::kUnknownProbabilityValue;
cells_[ToFlatIndex(cell_index, limits_)] != kUnknownProbabilityValue;
}
// Fills in 'offset' and 'limits' to define a subregion of that contains all
@ -126,7 +124,7 @@ void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) {
const int offset = x_offset + stride * y_offset;
const int new_size = new_limits.cell_limits().num_x_cells *
new_limits.cell_limits().num_y_cells;
std::vector<uint16> new_cells(new_size, mapping::kUnknownProbabilityValue);
std::vector<uint16> new_cells(new_size, kUnknownProbabilityValue);
for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
new_cells[offset + j + i * stride] =
@ -143,7 +141,7 @@ void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) {
proto::ProbabilityGrid ProbabilityGrid::ToProto() const {
proto::ProbabilityGrid result;
*result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_);
*result.mutable_limits() = mapping::ToProto(limits_);
result.mutable_cells()->Reserve(cells_.size());
for (const auto& cell : cells_) {
result.mutable_cells()->Add(cell);
@ -160,5 +158,5 @@ proto::ProbabilityGrid ProbabilityGrid::ToProto() const {
return result;
}
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

4
cartographer/mapping_2d/probability_grid.h
View File

@ -25,7 +25,7 @@
#include "cartographer/mapping_2d/xy_index.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
// Represents a 2D grid of probabilities.
class ProbabilityGrid {
@ -81,7 +81,7 @@ class ProbabilityGrid {
Eigen::AlignedBox2i known_cells_box_;
};
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_

38
cartographer/mapping_2d/probability_grid_test.cc
View File

@ -22,7 +22,7 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace {
using Eigen::Array2i;
@ -75,35 +75,29 @@ TEST(ProbabilityGridTest, ApplyOdds) {
probability_grid.SetProbability(Array2i(1, 0), 0.5);
probability_grid.ApplyLookupTable(
Array2i(1, 0),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
probability_grid.ApplyLookupTable(Array2i(1, 0),
ComputeLookupTableToApplyOdds(Odds(0.9)));
probability_grid.FinishUpdate();
EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5);
probability_grid.SetProbability(Array2i(0, 1), 0.5);
probability_grid.ApplyLookupTable(
Array2i(0, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1)));
probability_grid.ApplyLookupTable(Array2i(0, 1),
ComputeLookupTableToApplyOdds(Odds(0.1)));
probability_grid.FinishUpdate();
EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5);
// Tests adding odds to an unknown cell.
probability_grid.ApplyLookupTable(
Array2i(1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42)));
probability_grid.ApplyLookupTable(Array2i(1, 1),
ComputeLookupTableToApplyOdds(Odds(0.42)));
EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
// Tests that further updates are ignored if FinishUpdate() isn't called.
probability_grid.ApplyLookupTable(
Array2i(1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
probability_grid.ApplyLookupTable(Array2i(1, 1),
ComputeLookupTableToApplyOdds(Odds(0.9)));
EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
probability_grid.FinishUpdate();
probability_grid.ApplyLookupTable(
Array2i(1, 1),
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
probability_grid.ApplyLookupTable(Array2i(1, 1),
ComputeLookupTableToApplyOdds(Odds(0.9)));
EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42);
}
@ -120,10 +114,10 @@ TEST(ProbabilityGridTest, GetProbability) {
ASSERT_EQ(2, cell_limits.num_y_cells);
probability_grid.SetProbability(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)),
mapping::kMaxProbability);
kMaxProbability);
EXPECT_NEAR(probability_grid.GetProbability(
limits.GetCellIndex(Vector2f(-0.5f, 0.5f))),
mapping::kMaxProbability, 1e-6);
kMaxProbability, 1e-6);
for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)),
limits.GetCellIndex(Vector2f(0.5f, 0.5f)),
limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) {
@ -165,8 +159,8 @@ TEST(ProbabilityGridTest, GetCellIndex) {
TEST(ProbabilityGridTest, CorrectCropping) {
// Create a probability grid with random values.
std::mt19937 rng(42);
std::uniform_real_distribution<float> value_distribution(
mapping::kMinProbability, mapping::kMaxProbability);
std::uniform_real_distribution<float> value_distribution(kMinProbability,
kMaxProbability);
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
for (const Array2i& xy_index :
@ -182,5 +176,5 @@ TEST(ProbabilityGridTest, CorrectCropping) {
}
} // namespace
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

2
cartographer/mapping_2d/proto/cell_limits.proto
View File

@ -14,7 +14,7 @@
syntax = "proto3";
package cartographer.mapping_2d.proto;
package cartographer.mapping.proto;
message CellLimits {
int32 num_x_cells = 1;

20
cartographer/mapping_2d/proto/local_trajectory_builder_options.proto → cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.proto
View File

@ -14,15 +14,15 @@
syntax = "proto3";
package cartographer.mapping_2d.proto;
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.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";
message LocalTrajectoryBuilderOptions {
message LocalTrajectoryBuilderOptions2D {
// Rangefinder points outside these ranges will be dropped.
float min_range = 14;
float max_range = 15;
@ -41,8 +41,7 @@ message LocalTrajectoryBuilderOptions {
float voxel_filter_size = 3;
// Voxel filter used to compute a sparser point cloud for matching.
sensor.proto.AdaptiveVoxelFilterOptions
adaptive_voxel_filter_options = 6;
sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options = 6;
// Voxel filter used to compute a sparser point cloud for finding loop
// closures.
@ -52,11 +51,12 @@ message LocalTrajectoryBuilderOptions {
// 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;
scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 7;
scan_matching.proto.CeresScanMatcherOptions
cartographer.mapping_2d.scan_matching.proto
.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 7;
cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions
ceres_scan_matcher_options = 8;
mapping.proto.MotionFilterOptions motion_filter_options = 13;
MotionFilterOptions motion_filter_options = 13;
// Time constant in seconds for the orientation moving average based on
// observed gravity via the IMU. It should be chosen so that the error
@ -66,7 +66,7 @@ message LocalTrajectoryBuilderOptions {
// constant is increased) is balanced.
double imu_gravity_time_constant = 17;
mapping_2d.proto.SubmapsOptions submaps_options = 11;
SubmapsOptions2D submaps_options = 11;
// True if IMU data should be expected and used.
bool use_imu_data = 12;

2
cartographer/mapping_2d/proto/map_limits.proto
View File

@ -17,7 +17,7 @@ syntax = "proto3";
import "cartographer/mapping_2d/proto/cell_limits.proto";
import "cartographer/transform/proto/transform.proto";
package cartographer.mapping_2d.proto;
package cartographer.mapping.proto;
message MapLimits {
double resolution = 1;

2
cartographer/mapping_2d/proto/probability_grid.proto
View File

@ -16,7 +16,7 @@ syntax = "proto3";
import "cartographer/mapping_2d/proto/map_limits.proto";
package cartographer.mapping_2d.proto;
package cartographer.mapping.proto;
message ProbabilityGrid {
message CellBox {

4
cartographer/mapping_2d/proto/range_data_inserter_options.proto → cartographer/mapping_2d/proto/range_data_inserter_options_2d.proto
View File

@ -14,9 +14,9 @@
syntax = "proto3";
package cartographer.mapping_2d.proto;
package cartographer.mapping.proto;
message RangeDataInserterOptions {
message RangeDataInserterOptions2D {
// Probability change for a hit (this will be converted to odds and therefore
// must be greater than 0.5).
double hit_probability = 1;

8
cartographer/mapping_2d/proto/submaps_options.proto → cartographer/mapping_2d/proto/submaps_options_2d.proto
View File

@ -14,11 +14,11 @@
syntax = "proto3";
import "cartographer/mapping_2d/proto/range_data_inserter_options.proto";
import "cartographer/mapping_2d/proto/range_data_inserter_options_2d.proto";
package cartographer.mapping_2d.proto;
package cartographer.mapping.proto;
message SubmapsOptions {
message SubmapsOptions2D {
// Resolution of the map in meters.
double resolution = 1;
@ -27,5 +27,5 @@ message SubmapsOptions {
// matched against, then while being matched.
int32 num_range_data = 3;
RangeDataInserterOptions range_data_inserter_options = 5;
RangeDataInserterOptions2D range_data_inserter_options = 5;
}

27
cartographer/mapping_2d/range_data_inserter.cc → cartographer/mapping_2d/range_data_inserter_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_2d/range_data_inserter_2d.h"
#include <cstdlib>
@ -26,11 +26,11 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::RangeDataInserterOptions options;
proto::RangeDataInserterOptions2D options;
options.set_hit_probability(
parameter_dictionary->GetDouble("hit_probability"));
options.set_miss_probability(
@ -44,16 +44,17 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
return options;
}
RangeDataInserter::RangeDataInserter(
const proto::RangeDataInserterOptions& options)
RangeDataInserter2D::RangeDataInserter2D(
const proto::RangeDataInserterOptions2D& options)
: options_(options),
hit_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.hit_probability()))),
miss_table_(mapping::ComputeLookupTableToApplyOdds(
mapping::Odds(options.miss_probability()))) {}
hit_table_(
ComputeLookupTableToApplyOdds(Odds(options.hit_probability()))),
miss_table_(
ComputeLookupTableToApplyOdds(Odds(options.miss_probability()))) {}
void RangeDataInserter::Insert(const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) const {
void RangeDataInserter2D::Insert(
const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) const {
// By not finishing the update after hits are inserted, we give hits priority
// (i.e. no hits will be ignored because of a miss in the same cell).
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
@ -61,5 +62,5 @@ void RangeDataInserter::Insert(const sensor::RangeData& range_data,
probability_grid->FinishUpdate();
}
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

25
cartographer/mapping_2d/range_data_inserter.h → cartographer/mapping_2d/range_data_inserter_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_
#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_
#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_
#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_
#include <utility>
#include <vector>
@ -23,35 +23,36 @@
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
#include "cartographer/mapping_2d/proto/range_data_inserter_options_2d.pb.h"
#include "cartographer/mapping_2d/xy_index.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D(
common::LuaParameterDictionary* parameter_dictionary);
class RangeDataInserter {
class RangeDataInserter2D {
public:
explicit RangeDataInserter(const proto::RangeDataInserterOptions& options);
explicit RangeDataInserter2D(
const proto::RangeDataInserterOptions2D& options);
RangeDataInserter(const RangeDataInserter&) = delete;
RangeDataInserter& operator=(const RangeDataInserter&) = delete;
RangeDataInserter2D(const RangeDataInserter2D&) = delete;
RangeDataInserter2D& operator=(const RangeDataInserter2D&) = delete;
// Inserts 'range_data' into 'probability_grid'.
void Insert(const sensor::RangeData& range_data,
ProbabilityGrid* probability_grid) const;
private:
const proto::RangeDataInserterOptions options_;
const proto::RangeDataInserterOptions2D options_;
const std::vector<uint16> hit_table_;
const std::vector<uint16> miss_table_;
};
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_
#endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_

26
cartographer/mapping_2d/range_data_inserter_test.cc → cartographer/mapping_2d/range_data_inserter_2d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_2d/range_data_inserter_2d.h"
#include <memory>
@ -26,12 +26,12 @@
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace {
class RangeDataInserterTest : public ::testing::Test {
class RangeDataInserterTest2D : public ::testing::Test {
protected:
RangeDataInserterTest()
RangeDataInserterTest2D()
: probability_grid_(
MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) {
auto parameter_dictionary = common::MakeDictionary(
@ -40,8 +40,8 @@ class RangeDataInserterTest : public ::testing::Test {
"hit_probability = 0.7, "
"miss_probability = 0.4, "
"}");
options_ = CreateRangeDataInserterOptions(parameter_dictionary.get());
range_data_inserter_ = common::make_unique<RangeDataInserter>(options_);
options_ = CreateRangeDataInserterOptions2D(parameter_dictionary.get());
range_data_inserter_ = common::make_unique<RangeDataInserter2D>(options_);
}
void InsertPointCloud() {
@ -57,11 +57,11 @@ class RangeDataInserterTest : public ::testing::Test {
}
ProbabilityGrid probability_grid_;
std::unique_ptr<RangeDataInserter> range_data_inserter_;
proto::RangeDataInserterOptions options_;
std::unique_ptr<RangeDataInserter2D> range_data_inserter_;
proto::RangeDataInserterOptions2D options_;
};
TEST_F(RangeDataInserterTest, InsertPointCloud) {
TEST_F(RangeDataInserterTest2D, InsertPointCloud) {
InsertPointCloud();
EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9);
@ -101,7 +101,7 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) {
}
}
TEST_F(RangeDataInserterTest, ProbabilityProgression) {
TEST_F(RangeDataInserterTest2D, ProbabilityProgression) {
InsertPointCloud();
EXPECT_NEAR(
options_.hit_probability(),
@ -118,17 +118,17 @@ TEST_F(RangeDataInserterTest, ProbabilityProgression) {
InsertPointCloud();
}
EXPECT_NEAR(
mapping::kMaxProbability,
kMaxProbability,
probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex(
Eigen::Vector2f(-3.5f, 0.5f))),
1e-3);
EXPECT_NEAR(
mapping::kMinProbability,
kMinProbability,
probability_grid_.GetProbability(probability_grid_.limits().GetCellIndex(
Eigen::Vector2f(-2.5f, 0.5f))),
1e-3);
}
} // namespace
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

5
cartographer/mapping_2d/ray_casting.cc
View File

@ -17,8 +17,7 @@
#include "cartographer/mapping_2d/ray_casting.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace {
// Factor for subpixel accuracy of start and end point.
@ -203,5 +202,5 @@ void CastRays(const sensor::RangeData& range_data,
}
}
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

4
cartographer/mapping_2d/ray_casting.h
View File

@ -26,7 +26,7 @@
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
// For each ray in 'range_data', inserts hits and misses into
// 'probability_grid'. Hits are handled before misses.
@ -35,7 +35,7 @@ void CastRays(const sensor::RangeData& range_data,
const std::vector<uint16>& miss_table, bool insert_free_space,
ProbabilityGrid* probability_grid);
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_

2
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc
View File

@ -62,7 +62,7 @@ CeresScanMatcher::~CeresScanMatcher() {}
void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const {
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),

2
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h
View File

@ -49,7 +49,7 @@ class CeresScanMatcher {
void Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate,
ceres::Solver::Summary* summary) const;

6
cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc
View File

@ -35,8 +35,8 @@ namespace {
class CeresScanMatcherTest : public ::testing::Test {
protected:
CeresScanMatcherTest()
: probability_grid_(
MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) {
: probability_grid_(mapping::MapLimits(1., Eigen::Vector2d(10., 10.),
mapping::CellLimits(20, 20))) {
probability_grid_.SetProbability(
probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
mapping::kMaxProbability);
@ -73,7 +73,7 @@ class CeresScanMatcherTest : public ::testing::Test {
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
}
ProbabilityGrid probability_grid_;
mapping::ProbabilityGrid probability_grid_;
sensor::PointCloud point_cloud_;
std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_;
};

5
cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc
View File

@ -71,7 +71,7 @@ SearchParameters::SearchParameters(const int num_linear_perturbations,
}
void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan>& scans,
const CellLimits& cell_limits) {
const mapping::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,7 +109,8 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
}
std::vector<DiscreteScan> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const mapping::MapLimits& map_limits,
const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan> discrete_scans;
discrete_scans.reserve(scans.size());

5
cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h
View File

@ -50,7 +50,7 @@ struct SearchParameters {
// Tightens the search window as much as possible.
void ShrinkToFit(const std::vector<DiscreteScan>& scans,
const CellLimits& cell_limits);
const mapping::CellLimits& cell_limits);
int num_angular_perturbations;
double angular_perturbation_step_size;
@ -67,7 +67,8 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
// Translates and discretizes the rotated scans into a vector of integer
// indices.
std::vector<DiscreteScan> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const mapping::MapLimits& map_limits,
const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation);
// A possible solution.

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

@ -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 MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
CellLimits(6, 6));
const mapping::MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
mapping::CellLimits(6, 6));
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans =

11
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -89,8 +89,9 @@ CreateFastCorrelativeScanMatcherOptions(
}
PrecomputationGrid::PrecomputationGrid(
const ProbabilityGrid& probability_grid, const CellLimits& limits,
const int width, std::vector<float>* reusable_intermediate_grid)
const mapping::ProbabilityGrid& probability_grid,
const mapping::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),
@ -170,13 +171,13 @@ uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
class PrecomputationGridStack {
public:
PrecomputationGridStack(
const ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions& 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 CellLimits limits = probability_grid.limits().cell_limits();
const mapping::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,7 +198,7 @@ class PrecomputationGridStack {
};
FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions& options)
: options_(options),
limits_(probability_grid.limits()),

10
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h
View File

@ -49,8 +49,8 @@ CreateFastCorrelativeScanMatcherOptions(
// y0 <= y < y0.
class PrecomputationGrid {
public:
PrecomputationGrid(const ProbabilityGrid& probability_grid,
const CellLimits& limits, int width,
PrecomputationGrid(const mapping::ProbabilityGrid& probability_grid,
const mapping::CellLimits& limits, int width,
std::vector<float>* reusable_intermediate_grid);
// Returns a value between 0 and 255 to represent probabilities between
@ -87,7 +87,7 @@ class PrecomputationGrid {
const Eigen::Array2i offset_;
// Size of the precomputation grid.
const CellLimits wide_limits_;
const mapping::CellLimits wide_limits_;
// Probabilites mapped to 0 to 255.
std::vector<uint8> cells_;
@ -99,7 +99,7 @@ class PrecomputationGridStack;
class FastCorrelativeScanMatcher {
public:
FastCorrelativeScanMatcher(
const ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions& options);
~FastCorrelativeScanMatcher();
@ -146,7 +146,7 @@ class FastCorrelativeScanMatcher {
int candidate_depth, float min_score) const;
const proto::FastCorrelativeScanMatcherOptions options_;
MapLimits limits_;
mapping::MapLimits limits_;
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
};

44
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -24,7 +24,7 @@
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_2d/range_data_inserter_2d.h"
#include "cartographer/transform/rigid_transform_test_helpers.h"
#include "cartographer/transform/transform.h"
#include "gtest/gtest.h"
@ -39,10 +39,10 @@ TEST(PrecomputationGridTest, CorrectValues) {
// represented by uint8 values.
std::mt19937 prng(42);
std::uniform_int_distribution<int> distribution(0, 255);
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))) {
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))) {
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 :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
for (const Eigen::Array2i& xy_index : mapping::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);
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
0.05, Eigen::Vector2d(0.1, 0.1), mapping::CellLimits(4, 4)));
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
mapping::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 :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
for (const Eigen::Array2i& xy_index : mapping::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) {
@ -116,21 +116,22 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
}
mapping_2d::proto::RangeDataInserterOptions
CreateRangeDataInserterTestOptions() {
mapping::proto::RangeDataInserterOptions2D
CreateRangeDataInserterTestOptions2D() {
auto parameter_dictionary = common::MakeDictionary(R"text(
return {
insert_free_space = true,
hit_probability = 0.7,
miss_probability = 0.4,
})text");
return mapping_2d::CreateRangeDataInserterOptions(parameter_dictionary.get());
return mapping::CreateRangeDataInserterOptions2D(parameter_dictionary.get());
}
TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
std::mt19937 prng(42);
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
mapping::RangeDataInserter2D range_data_inserter(
CreateRangeDataInserterTestOptions2D());
constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(3);
@ -147,8 +148,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
{2. * distribution(prng), 2. * distribution(prng)},
0.5 * distribution(prng));
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200)));
range_data_inserter.Insert(
sensor::RangeData{
Eigen::Vector3f(expected_pose.translation().x(),
@ -177,7 +178,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
std::mt19937 prng(42);
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
mapping::RangeDataInserter2D range_data_inserter(
CreateRangeDataInserterTestOptions2D());
constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(6);
@ -200,8 +202,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
0.5 * distribution(prng)) *
perturbation.inverse();
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200)));
range_data_inserter.Insert(
sensor::RangeData{
transform::Embed3D(expected_pose * perturbation).translation(),

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

@ -91,7 +91,7 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
double RealTimeCorrelativeScanMatcher::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate);
@ -125,7 +125,7 @@ double RealTimeCorrelativeScanMatcher::Match(
}
void RealTimeCorrelativeScanMatcher::ScoreCandidates(
const ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* const candidates) const {

4
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h
View File

@ -69,7 +69,7 @@ class RealTimeCorrelativeScanMatcher {
// returns the score.
double Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
const mapping::ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const;
// Computes the score for each Candidate in a collection. The cost is computed
@ -77,7 +77,7 @@ class RealTimeCorrelativeScanMatcher {
// http://ceres-solver.org/modeling.html
//
// Visible for testing.
void ScoreCandidates(const ProbabilityGrid& probability_grid,
void ScoreCandidates(const mapping::ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* candidates) const;

15
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -23,7 +23,7 @@
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_2d/range_data_inserter_2d.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h"
#include "gtest/gtest.h"
@ -36,8 +36,8 @@ namespace {
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
protected:
RealTimeCorrelativeScanMatcherTest()
: probability_grid_(
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
: probability_grid_(mapping::MapLimits(0.05, Eigen::Vector2d(0.05, 0.25),
mapping::CellLimits(6, 6))) {
{
auto parameter_dictionary = common::MakeDictionary(
"return { "
@ -45,8 +45,9 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
"hit_probability = 0.7, "
"miss_probability = 0.4, "
"}");
range_data_inserter_ = common::make_unique<RangeDataInserter>(
CreateRangeDataInserterOptions(parameter_dictionary.get()));
range_data_inserter_ = common::make_unique<mapping::RangeDataInserter2D>(
mapping::CreateRangeDataInserterOptions2D(
parameter_dictionary.get()));
}
point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
@ -74,8 +75,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
}
}
ProbabilityGrid probability_grid_;
std::unique_ptr<RangeDataInserter> range_data_inserter_;
mapping::ProbabilityGrid probability_grid_;
std::unique_ptr<mapping::RangeDataInserter2D> range_data_inserter_;
sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher>
real_time_correlative_scan_matcher_;

56
cartographer/mapping_2d/submaps.cc → cartographer/mapping_2d/submap_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include <cinttypes>
#include <cmath>
@ -28,7 +28,7 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
ProbabilityGrid ComputeCroppedProbabilityGrid(
const ProbabilityGrid& probability_grid) {
@ -49,33 +49,33 @@ ProbabilityGrid ComputeCroppedProbabilityGrid(
return cropped_grid;
}
proto::SubmapsOptions CreateSubmapsOptions(
proto::SubmapsOptions2D CreateSubmapsOptions2D(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::SubmapsOptions options;
proto::SubmapsOptions2D options;
options.set_resolution(parameter_dictionary->GetDouble("resolution"));
options.set_num_range_data(
parameter_dictionary->GetNonNegativeInt("num_range_data"));
*options.mutable_range_data_inserter_options() =
CreateRangeDataInserterOptions(
CreateRangeDataInserterOptions2D(
parameter_dictionary->GetDictionary("range_data_inserter").get());
CHECK_GT(options.num_range_data(), 0);
return options;
}
Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
: mapping::Submap(transform::Rigid3d::Translation(
Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin)
: Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
probability_grid_(limits) {}
Submap::Submap(const mapping::proto::Submap2D& proto)
: mapping::Submap(transform::ToRigid3(proto.local_pose())),
Submap2D::Submap2D(const proto::Submap2D& proto)
: Submap(transform::ToRigid3(proto.local_pose())),
probability_grid_(ProbabilityGrid(proto.probability_grid())) {
SetNumRangeData(proto.num_range_data());
SetFinished(proto.finished());
}
void Submap::ToProto(mapping::proto::Submap* const proto,
bool include_probability_grid_data) const {
void Submap2D::ToProto(proto::Submap* const proto,
bool include_probability_grid_data) const {
auto* const submap_2d = proto->mutable_submap_2d();
*submap_2d->mutable_local_pose() = transform::ToProto(local_pose());
submap_2d->set_num_range_data(num_range_data());
@ -85,7 +85,7 @@ void Submap::ToProto(mapping::proto::Submap* const proto,
}
}
void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
void Submap2D::UpdateFromProto(const proto::Submap& proto) {
CHECK(proto.has_submap_2d());
const auto& submap_2d = proto.submap_2d();
SetNumRangeData(submap_2d.num_range_data());
@ -95,9 +95,9 @@ void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
}
}
void Submap::ToResponseProto(
void Submap2D::ToResponseProto(
const transform::Rigid3d&,
mapping::proto::SubmapQuery::Response* const response) const {
proto::SubmapQuery::Response* const response) const {
response->set_submap_version(num_range_data());
Eigen::Array2i offset;
@ -114,7 +114,7 @@ void Submap::ToResponseProto(
// is currently white, so walls will look too gray. This should be hard to
// detect visually for the user, though.
const int delta =
128 - mapping::ProbabilityToLogOddsInteger(
128 - ProbabilityToLogOddsInteger(
probability_grid_.GetProbability(xy_index + offset));
const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0;
@ -126,7 +126,7 @@ void Submap::ToResponseProto(
cells.push_back(0); // alpha
}
}
mapping::proto::SubmapQuery::Response::SubmapTexture* const texture =
proto::SubmapQuery::Response::SubmapTexture* const texture =
response->add_textures();
common::FastGzipString(cells, texture->mutable_cells());
@ -143,20 +143,20 @@ void Submap::ToResponseProto(
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
}
void Submap::InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter& range_data_inserter) {
void Submap2D::InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter2D& range_data_inserter) {
CHECK(!finished());
range_data_inserter.Insert(range_data, &probability_grid_);
SetNumRangeData(num_range_data() + 1);
}
void Submap::Finish() {
void Submap2D::Finish() {
CHECK(!finished());
probability_grid_ = ComputeCroppedProbabilityGrid(probability_grid_);
SetFinished(true);
}
ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
: options_(options),
range_data_inserter_(options.range_data_inserter_options()) {
// We always want to have at least one likelihood field which we can return,
@ -164,7 +164,7 @@ ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
AddSubmap(Eigen::Vector2f::Zero());
}
void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) {
void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) {
for (auto& submap : submaps_) {
submap->InsertRangeData(range_data, range_data_inserter_);
}
@ -173,27 +173,27 @@ void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) {
}
}
std::vector<std::shared_ptr<Submap>> ActiveSubmaps::submaps() const {
std::vector<std::shared_ptr<Submap2D>> ActiveSubmaps2D::submaps() const {
return submaps_;
}
int ActiveSubmaps::matching_index() const { return matching_submap_index_; }
int ActiveSubmaps2D::matching_index() const { return matching_submap_index_; }
void ActiveSubmaps::FinishSubmap() {
Submap* submap = submaps_.front().get();
void ActiveSubmaps2D::FinishSubmap() {
Submap2D* submap = submaps_.front().get();
submap->Finish();
++matching_submap_index_;
submaps_.erase(submaps_.begin());
}
void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) {
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
if (submaps_.size() > 1) {
// This will crop the finished Submap before inserting a new Submap to
// reduce peak memory usage a bit.
FinishSubmap();
}
constexpr int kInitialSubmapSize = 100;
submaps_.push_back(common::make_unique<Submap>(
submaps_.push_back(common::make_unique<Submap2D>(
MapLimits(options_.resolution(),
origin.cast<double>() + 0.5 * kInitialSubmapSize *
options_.resolution() *
@ -203,5 +203,5 @@ void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) {
LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size();
}
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

49
cartographer/mapping_2d/submaps.h → cartographer/mapping_2d/submap_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_
#define CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_
#ifndef CARTOGRAPHER_MAPPING_2D_SUBMAP_2D_H_
#define CARTOGRAPHER_MAPPING_2D_SUBMAP_2D_H_
#include <memory>
#include <vector>
@ -28,39 +28,38 @@
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_2d/map_limits.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/proto/submaps_options.pb.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_2d/proto/submaps_options_2d.pb.h"
#include "cartographer/mapping_2d/range_data_inserter_2d.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
ProbabilityGrid ComputeCroppedProbabilityGrid(
const ProbabilityGrid& probability_grid);
proto::SubmapsOptions CreateSubmapsOptions(
proto::SubmapsOptions2D CreateSubmapsOptions2D(
common::LuaParameterDictionary* parameter_dictionary);
class Submap : public mapping::Submap {
class Submap2D : public Submap {
public:
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
explicit Submap(const mapping::proto::Submap2D& proto);
Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin);
explicit Submap2D(const proto::Submap2D& proto);
void ToProto(mapping::proto::Submap* proto,
void ToProto(proto::Submap* proto,
bool include_probability_grid_data) const override;
void UpdateFromProto(const mapping::proto::Submap& proto) override;
void UpdateFromProto(const proto::Submap& proto) override;
void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override;
void ToResponseProto(const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) const override;
const ProbabilityGrid& probability_grid() const { return probability_grid_; }
// Insert 'range_data' into this submap using 'range_data_inserter'. The
// submap must not be finished yet.
void InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter& range_data_inserter);
const RangeDataInserter2D& range_data_inserter);
void Finish();
private:
@ -76,12 +75,12 @@ class Submap : public mapping::Submap {
// considered initialized: the old submap is no longer changed, the "new" submap
// is now the "old" submap and is used for scan-to-map matching. Moreover, a
// "new" submap gets created. The "old" submap is forgotten by this object.
class ActiveSubmaps {
class ActiveSubmaps2D {
public:
explicit ActiveSubmaps(const proto::SubmapsOptions& options);
explicit ActiveSubmaps2D(const proto::SubmapsOptions2D& options);
ActiveSubmaps(const ActiveSubmaps&) = delete;
ActiveSubmaps& operator=(const ActiveSubmaps&) = delete;
ActiveSubmaps2D(const ActiveSubmaps2D&) = delete;
ActiveSubmaps2D& operator=(const ActiveSubmaps2D&) = delete;
// Returns the index of the newest initialized Submap which can be
// used for scan-to-map matching.
@ -90,19 +89,19 @@ class ActiveSubmaps {
// Inserts 'range_data' into the Submap collection.
void InsertRangeData(const sensor::RangeData& range_data);
std::vector<std::shared_ptr<Submap>> submaps() const;
std::vector<std::shared_ptr<Submap2D>> submaps() const;
private:
void FinishSubmap();
void AddSubmap(const Eigen::Vector2f& origin);
const proto::SubmapsOptions options_;
const proto::SubmapsOptions2D options_;
int matching_submap_index_ = 0;
std::vector<std::shared_ptr<Submap>> submaps_;
RangeDataInserter range_data_inserter_;
std::vector<std::shared_ptr<Submap2D>> submaps_;
RangeDataInserter2D range_data_inserter_;
};
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_
#endif // CARTOGRAPHER_MAPPING_2D_SUBMAP_2D_H_

23
cartographer/mapping_2d/submaps_test.cc → cartographer/mapping_2d/submap_2d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include <map>
#include <memory>
@ -28,10 +28,10 @@
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace {
TEST(SubmapsTest, TheRightNumberOfRangeDataAreInserted) {
TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
constexpr int kNumRangeData = 10;
auto parameter_dictionary = common::MakeDictionary(
"return {"
@ -45,8 +45,8 @@ TEST(SubmapsTest, TheRightNumberOfRangeDataAreInserted) {
"miss_probability = 0.495, "
"},"
"}");
ActiveSubmaps submaps{CreateSubmapsOptions(parameter_dictionary.get())};
std::set<std::shared_ptr<Submap>> all_submaps;
ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())};
std::set<std::shared_ptr<Submap2D>> all_submaps;
for (int i = 0; i != 1000; ++i) {
submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}});
// Except for the first, maps should only be returned after enough range
@ -68,14 +68,15 @@ TEST(SubmapsTest, TheRightNumberOfRangeDataAreInserted) {
EXPECT_EQ(correct_num_range_data, all_submaps.size() - 2);
}
TEST(SubmapsTest, ToFromProto) {
Submap expected(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)),
Eigen::Vector2f(4.f, 5.f));
mapping::proto::Submap proto;
TEST(Submap2DTest, ToFromProto) {
Submap2D expected(
MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)),
Eigen::Vector2f(4.f, 5.f));
proto::Submap proto;
expected.ToProto(&proto, true /* include_probability_grid_data */);
EXPECT_TRUE(proto.has_submap_2d());
EXPECT_FALSE(proto.has_submap_3d());
const auto actual = Submap(proto.submap_2d());
const auto actual = Submap2D(proto.submap_2d());
EXPECT_TRUE(expected.local_pose().translation().isApprox(
actual.local_pose().translation(), 1e-6));
EXPECT_TRUE(expected.local_pose().rotation().isApprox(
@ -91,5 +92,5 @@ TEST(SubmapsTest, ToFromProto) {
}
} // namespace
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

4
cartographer/mapping_2d/xy_index.h
View File

@ -29,7 +29,7 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
struct CellLimits {
CellLimits() = default;
@ -108,7 +108,7 @@ class XYIndexRangeIterator
Eigen::Array2i xy_index_;
};
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_

4
cartographer/mapping_2d/xy_index_test.cc
View File

@ -19,7 +19,7 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_2d {
namespace mapping {
namespace {
TEST(XYIndexTest, CellLimitsToProto) {
@ -57,5 +57,5 @@ TEST(XYIndexTest, XYIndexRangeIterator) {
}
} // namespace
} // namespace mapping_2d
} // namespace mapping
} // namespace cartographer

1
cartographer/mapping_3d/submaps.h
View File

@ -27,7 +27,6 @@
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/proto/submaps_options.pb.h"
#include "cartographer/mapping_3d/range_data_inserter.h"

11
cartographer_grpc/map_builder_context.cc
View File

@ -70,22 +70,22 @@ void MapBuilderContext::NotifyFinishTrajectory(int trajectory_id) {
map_builder_server_->NotifyFinishTrajectory(trajectory_id);
}
std::shared_ptr<cartographer::mapping_2d::Submap>
std::shared_ptr<cartographer::mapping::Submap2D>
MapBuilderContext::UpdateSubmap2D(
const cartographer::mapping::proto::Submap& proto) {
CHECK(proto.has_submap_2d());
cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
proto.submap_id().submap_index()};
std::shared_ptr<cartographer::mapping_2d::Submap> submap_2d_ptr;
std::shared_ptr<cartographer::mapping::Submap2D> submap_2d_ptr;
auto submap_it = unfinished_submaps_.find(submap_id);
if (submap_it == unfinished_submaps_.end()) {
// Seeing a submap for the first time it should never be finished.
CHECK(!proto.submap_2d().finished());
submap_2d_ptr =
std::make_shared<cartographer::mapping_2d::Submap>(proto.submap_2d());
std::make_shared<cartographer::mapping::Submap2D>(proto.submap_2d());
unfinished_submaps_.Insert(submap_id, submap_2d_ptr);
} else {
submap_2d_ptr = std::dynamic_pointer_cast<cartographer::mapping_2d::Submap>(
submap_2d_ptr = std::dynamic_pointer_cast<cartographer::mapping::Submap2D>(
submap_it->data);
CHECK(submap_2d_ptr);
submap_2d_ptr->UpdateFromProto(proto);
@ -146,8 +146,7 @@ MapBuilderContext::ProcessLocalSlamResultData(
CHECK_GE(proto.submaps().size(), 1);
CHECK(proto.submaps(0).has_submap_2d() || proto.submaps(0).has_submap_3d());
if (proto.submaps(0).has_submap_2d()) {
std::vector<std::shared_ptr<const cartographer::mapping_2d::Submap>>
submaps;
std::vector<std::shared_ptr<const cartographer::mapping::Submap2D>> submaps;
for (const auto& submap_proto : proto.submaps()) {
submaps.push_back(UpdateSubmap2D(submap_proto));
}

4
cartographer_grpc/map_builder_context.h
View File

@ -17,7 +17,7 @@
#ifndef CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H
#define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/mapping_3d/submaps.h"
#include "cartographer_grpc/map_builder_context_interface.h"
@ -51,7 +51,7 @@ class MapBuilderContext : public MapBuilderContextInterface {
local_slam_result_data) override;
private:
std::shared_ptr<cartographer::mapping_2d::Submap> UpdateSubmap2D(
std::shared_ptr<cartographer::mapping::Submap2D> UpdateSubmap2D(
const cartographer::mapping::proto::Submap& proto);
std::shared_ptr<cartographer::mapping_3d::Submap> UpdateSubmap3D(
const cartographer::mapping::proto::Submap& proto);