Remove 'mapping_3d' namespace. (#922) (#925)

Remove 'mapping_3d' namespace. (#922)

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

[Code structure RFC](e11bca586f/text/0000-code-structure.md)
master
Alexander Belyaev 2018-02-21 14:24:12 +01:00 committed by GitHub
parent f8dc89d8ff
commit 7d13383dec
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
56 changed files with 296 additions and 298 deletions

View File

@ -21,8 +21,8 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
#include "cartographer/mapping_3d/proto/submaps_options.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" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -31,7 +31,7 @@ namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
LocalTrajectoryBuilder::LocalTrajectoryBuilder( LocalTrajectoryBuilder::LocalTrajectoryBuilder(
const proto::LocalTrajectoryBuilderOptions& options) const mapping::proto::LocalTrajectoryBuilderOptions3D& options)
: options_(options), : options_(options),
active_submaps_(options.submaps_options()), active_submaps_(options.submaps_options()),
motion_filter_(options.motion_filter_options()), motion_filter_(options.motion_filter_options()),
@ -142,7 +142,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
const transform::Rigid3d pose_prediction = const transform::Rigid3d pose_prediction =
extrapolator_->ExtrapolatePose(time); extrapolator_->ExtrapolatePose(time);
std::shared_ptr<const Submap> matching_submap = std::shared_ptr<const mapping::Submap3D> matching_submap =
active_submaps_.submaps().front(); active_submaps_.submaps().front();
transform::Rigid3d initial_ceres_pose = transform::Rigid3d initial_ceres_pose =
matching_submap->local_pose().inverse() * pose_prediction; matching_submap->local_pose().inverse() * pose_prediction;
@ -223,8 +223,9 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
} }
// Querying the active submaps must be done here before calling // Querying the active submaps must be done here before calling
// InsertRangeData() since the queried values are valid for next insertion. // InsertRangeData() since the queried values are valid for next insertion.
std::vector<std::shared_ptr<const Submap>> insertion_submaps; std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps;
for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps()) { for (const std::shared_ptr<mapping::Submap3D>& submap :
active_submaps_.submaps()) {
insertion_submaps.push_back(submap); insertion_submaps.push_back(submap);
} }
active_submaps_.InsertRangeData(filtered_range_data_in_local, active_submaps_.InsertRangeData(filtered_range_data_in_local,

View File

@ -22,10 +22,10 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/internal/mapping/motion_filter.h" #include "cartographer/internal/mapping/motion_filter.h"
#include "cartographer/mapping/pose_extrapolator.h" #include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
@ -41,7 +41,7 @@ class LocalTrajectoryBuilder {
public: public:
struct InsertionResult { struct InsertionResult {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data; 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::Submap3D>> insertion_submaps;
}; };
struct MatchingResult { struct MatchingResult {
common::Time time; common::Time time;
@ -52,7 +52,7 @@ class LocalTrajectoryBuilder {
}; };
explicit LocalTrajectoryBuilder( explicit LocalTrajectoryBuilder(
const proto::LocalTrajectoryBuilderOptions& options); const mapping::proto::LocalTrajectoryBuilderOptions3D& options);
~LocalTrajectoryBuilder(); ~LocalTrajectoryBuilder();
LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
@ -80,8 +80,8 @@ class LocalTrajectoryBuilder {
const transform::Rigid3d& pose_estimate, const transform::Rigid3d& pose_estimate,
const Eigen::Quaterniond& gravity_alignment); const Eigen::Quaterniond& gravity_alignment);
const proto::LocalTrajectoryBuilderOptions options_; const mapping::proto::LocalTrajectoryBuilderOptions3D options_;
ActiveSubmaps active_submaps_; mapping::ActiveSubmaps3D active_submaps_;
mapping::MotionFilter motion_filter_; mapping::MotionFilter motion_filter_;
std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher> std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>

View File

@ -23,7 +23,7 @@
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h" #include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/rigid_transform_test_helpers.h"
@ -44,7 +44,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
void SetUp() override { GenerateBubbles(); } void SetUp() override { GenerateBubbles(); }
proto::LocalTrajectoryBuilderOptions CreateTrajectoryBuilderOptions() { mapping::proto::LocalTrajectoryBuilderOptions3D
CreateTrajectoryBuilderOptions3D() {
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
min_range = 0.5, min_range = 0.5,
@ -107,7 +108,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
}, },
} }
)text"); )text");
return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get()); return mapping::CreateLocalTrajectoryBuilderOptions3D(
parameter_dictionary.get());
} }
void GenerateBubbles() { void GenerateBubbles() {
@ -270,7 +272,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) { TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) {
local_trajectory_builder_.reset( local_trajectory_builder_.reset(
new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions())); new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions3D()));
VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1); VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1);
} }

View File

@ -35,7 +35,7 @@ class OccupiedSpaceCostFunction {
public: public:
static ceres::CostFunction* CreateAutoDiffCostFunction( static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud, const double scaling_factor, const sensor::PointCloud& point_cloud,
const HybridGrid& hybrid_grid) { const mapping::HybridGrid& hybrid_grid) {
return new ceres::AutoDiffCostFunction< return new ceres::AutoDiffCostFunction<
OccupiedSpaceCostFunction, ceres::DYNAMIC /* residuals */, OccupiedSpaceCostFunction, ceres::DYNAMIC /* residuals */,
3 /* translation variables */, 4 /* rotation variables */>( 3 /* translation variables */, 4 /* rotation variables */>(
@ -56,7 +56,7 @@ class OccupiedSpaceCostFunction {
private: private:
OccupiedSpaceCostFunction(const double scaling_factor, OccupiedSpaceCostFunction(const double scaling_factor,
const sensor::PointCloud& point_cloud, const sensor::PointCloud& point_cloud,
const HybridGrid& hybrid_grid) const mapping::HybridGrid& hybrid_grid)
: scaling_factor_(scaling_factor), : scaling_factor_(scaling_factor),
point_cloud_(point_cloud), point_cloud_(point_cloud),
interpolated_grid_(hybrid_grid) {} interpolated_grid_(hybrid_grid) {}

View File

@ -9,7 +9,7 @@
#include "cartographer/io/points_batch.h" #include "cartographer/io/points_batch.h"
#include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor.h"
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/mapping_3d/range_data_inserter_3d.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -18,7 +18,7 @@ namespace io {
HybridGridPointsProcessor::HybridGridPointsProcessor( HybridGridPointsProcessor::HybridGridPointsProcessor(
const double voxel_size, const double voxel_size,
const mapping_3d::proto::RangeDataInserterOptions& const mapping::proto::RangeDataInserterOptions3D&
range_data_inserter_options, range_data_inserter_options,
std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next) std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
: next_(next), : next_(next),
@ -33,7 +33,7 @@ HybridGridPointsProcessor::FromDictionary(
PointsProcessor* const next) { PointsProcessor* const next) {
return common::make_unique<HybridGridPointsProcessor>( return common::make_unique<HybridGridPointsProcessor>(
dictionary->GetDouble("voxel_size"), dictionary->GetDouble("voxel_size"),
mapping_3d::CreateRangeDataInserterOptions( mapping::CreateRangeDataInserterOptions3D(
dictionary->GetDictionary("range_data_inserter").get()), dictionary->GetDictionary("range_data_inserter").get()),
file_writer_factory(dictionary->GetString("filename")), next); file_writer_factory(dictionary->GetString("filename")), next);
} }
@ -45,8 +45,7 @@ void HybridGridPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
} }
PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() { PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() {
const mapping_3d::proto::HybridGrid hybrid_grid_proto = const mapping::proto::HybridGrid hybrid_grid_proto = hybrid_grid_.ToProto();
hybrid_grid_.ToProto();
std::string serialized; std::string serialized;
hybrid_grid_proto.SerializeToString(&serialized); hybrid_grid_proto.SerializeToString(&serialized);
file_writer_->Write(serialized.data(), serialized.size()); file_writer_->Write(serialized.data(), serialized.size());

View File

@ -10,8 +10,8 @@
#include "cartographer/io/points_batch.h" #include "cartographer/io/points_batch.h"
#include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor.h"
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h" #include "cartographer/mapping_3d/proto/range_data_inserter_options_3d.pb.h"
#include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/mapping_3d/range_data_inserter_3d.h"
namespace cartographer { namespace cartographer {
namespace io { namespace io {
@ -24,7 +24,7 @@ class HybridGridPointsProcessor : public PointsProcessor {
constexpr static const char* kConfigurationFileActionName = constexpr static const char* kConfigurationFileActionName =
"write_hybrid_grid"; "write_hybrid_grid";
HybridGridPointsProcessor(double voxel_size, HybridGridPointsProcessor(double voxel_size,
const mapping_3d::proto::RangeDataInserterOptions& const mapping::proto::RangeDataInserterOptions3D&
range_data_inserter_options, range_data_inserter_options,
std::unique_ptr<FileWriter> file_writer, std::unique_ptr<FileWriter> file_writer,
PointsProcessor* next); PointsProcessor* next);
@ -44,8 +44,8 @@ class HybridGridPointsProcessor : public PointsProcessor {
private: private:
PointsProcessor* const next_; PointsProcessor* const next_;
mapping_3d::RangeDataInserter range_data_inserter_; mapping::RangeDataInserter3D range_data_inserter_;
mapping_3d::HybridGrid hybrid_grid_; mapping::HybridGrid hybrid_grid_;
std::unique_ptr<FileWriter> file_writer_; std::unique_ptr<FileWriter> file_writer_;
}; };

View File

@ -78,7 +78,7 @@ class OutlierRemovingPointsProcessor : public PointsProcessor {
const double voxel_size_; const double voxel_size_;
PointsProcessor* const next_; PointsProcessor* const next_;
State state_; State state_;
mapping_3d::HybridGridBase<VoxelData> voxels_; mapping::HybridGridBase<VoxelData> voxels_;
}; };
} // namespace io } // namespace io

View File

@ -17,7 +17,7 @@
#include "cartographer/io/submap_painter.h" #include "cartographer/io/submap_painter.h"
#include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submap_3d.h"
namespace cartographer { namespace cartographer {
namespace io { namespace io {
@ -114,7 +114,7 @@ void FillSubmapSlice(
::cartographer::mapping::proto::SubmapQuery::Response response; ::cartographer::mapping::proto::SubmapQuery::Response response;
::cartographer::transform::Rigid3d local_pose; ::cartographer::transform::Rigid3d local_pose;
if (proto.has_submap_3d()) { if (proto.has_submap_3d()) {
::cartographer::mapping_3d::Submap submap(proto.submap_3d()); mapping::Submap3D submap(proto.submap_3d());
local_pose = submap.local_pose(); local_pose = submap.local_pose();
submap.ToResponseProto(global_submap_pose, &response); submap.ToResponseProto(global_submap_pose, &response);
} else { } else {

View File

@ -111,7 +111,7 @@ XRayPointsProcessor::XRayPointsProcessor(
transform_(transform) { transform_(transform) {
for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) { for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
aggregations_.emplace_back( aggregations_.emplace_back(
Aggregation{mapping_3d::HybridGridBase<bool>(voxel_size), {}}); Aggregation{mapping::HybridGridBase<bool>(voxel_size), {}});
} }
} }
@ -161,7 +161,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
const int xsize = bounding_box_.sizes()[1] + 1; const int xsize = bounding_box_.sizes()[1] + 1;
const int ysize = bounding_box_.sizes()[2] + 1; const int ysize = bounding_box_.sizes()[2] + 1;
PixelDataMatrix pixel_data_matrix = PixelDataMatrix(ysize, xsize); PixelDataMatrix pixel_data_matrix = PixelDataMatrix(ysize, xsize);
for (mapping_3d::HybridGridBase<bool>::Iterator it(aggregation.voxels); for (mapping::HybridGridBase<bool>::Iterator it(aggregation.voxels);
!it.Done(); it.Next()) { !it.Done(); it.Next()) {
const Eigen::Array3i cell_index = it.GetCellIndex(); const Eigen::Array3i cell_index = it.GetCellIndex();
const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index); const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index);

View File

@ -66,7 +66,7 @@ class XRayPointsProcessor : public PointsProcessor {
}; };
struct Aggregation { struct Aggregation {
mapping_3d::HybridGridBase<bool> voxels; mapping::HybridGridBase<bool> voxels;
std::map<std::pair<int, int>, ColumnData> column_data; std::map<std::pair<int, int>, ColumnData> column_data;
}; };

View File

@ -49,7 +49,7 @@ void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
LocalSlamResult3D::LocalSlamResult3D( LocalSlamResult3D::LocalSlamResult3D(
const std::string& sensor_id, common::Time time, const std::string& sensor_id, common::Time time,
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data, std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const mapping_3d::Submap>>& const std::vector<std::shared_ptr<const mapping::Submap3D>>&
insertion_submaps) insertion_submaps)
: LocalSlamResultData(sensor_id, time), : LocalSlamResultData(sensor_id, time),
node_data_(node_data), node_data_(node_data),

View File

@ -58,8 +58,7 @@ class LocalSlamResult3D : public LocalSlamResultData {
LocalSlamResult3D( LocalSlamResult3D(
const std::string& sensor_id, common::Time time, const std::string& sensor_id, common::Time time,
std::shared_ptr<const TrajectoryNode::Data> node_data, std::shared_ptr<const TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const mapping_3d::Submap>>& const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps);
insertion_submaps);
void AddToTrajectoryBuilder( void AddToTrajectoryBuilder(
TrajectoryBuilderInterface* const trajectory_builder) override; TrajectoryBuilderInterface* const trajectory_builder) override;
@ -67,7 +66,7 @@ class LocalSlamResult3D : public LocalSlamResultData {
private: private:
std::shared_ptr<const TrajectoryNode::Data> node_data_; std::shared_ptr<const TrajectoryNode::Data> node_data_;
std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps_; std::vector<std::shared_ptr<const Submap3D>> insertion_submaps_;
}; };
} // namespace mapping } // namespace mapping

View File

@ -25,7 +25,7 @@ message Submap2D {
transform.proto.Rigid3d local_pose = 1; transform.proto.Rigid3d local_pose = 1;
int32 num_range_data = 2; int32 num_range_data = 2;
bool finished = 3; bool finished = 3;
mapping.proto.ProbabilityGrid probability_grid = 4; ProbabilityGrid probability_grid = 4;
} }
// Serialized state of a mapping_3d::Submap. // Serialized state of a mapping_3d::Submap.
@ -33,6 +33,6 @@ message Submap3D {
transform.proto.Rigid3d local_pose = 1; transform.proto.Rigid3d local_pose = 1;
int32 num_range_data = 2; int32 num_range_data = 2;
bool finished = 3; bool finished = 3;
mapping_3d.proto.HybridGrid high_resolution_hybrid_grid = 4; HybridGrid high_resolution_hybrid_grid = 4;
mapping_3d.proto.HybridGrid low_resolution_hybrid_grid = 5; HybridGrid low_resolution_hybrid_grid = 5;
} }

View File

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

View File

@ -18,7 +18,7 @@
#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h" #include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h" #include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
@ -30,7 +30,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
CreateLocalTrajectoryBuilderOptions2D( CreateLocalTrajectoryBuilderOptions2D(
parameter_dictionary->GetDictionary("trajectory_builder_2d").get()); parameter_dictionary->GetDictionary("trajectory_builder_2d").get());
*options.mutable_trajectory_builder_3d_options() = *options.mutable_trajectory_builder_3d_options() =
mapping_3d::CreateLocalTrajectoryBuilderOptions( CreateLocalTrajectoryBuilderOptions3D(
parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
options.set_pure_localization( options.set_pure_localization(
parameter_dictionary->GetBool("pure_localization")); parameter_dictionary->GetBool("pure_localization"));

View File

@ -33,7 +33,7 @@
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
// Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat // Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat
// z-major index. // z-major index.
@ -475,20 +475,20 @@ class HybridGrid : public HybridGridBase<uint16> {
// SetProbability does some error checking for us. // SetProbability does some error checking for us.
SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i), SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i),
proto.z_indices(i)), proto.z_indices(i)),
mapping::ValueToProbability(proto.values(i))); ValueToProbability(proto.values(i)));
} }
} }
// Sets the probability of the cell at 'index' to the given 'probability'. // Sets the probability of the cell at 'index' to the given 'probability'.
void SetProbability(const Eigen::Array3i& index, const float probability) { void SetProbability(const Eigen::Array3i& index, const float probability) {
*mutable_value(index) = mapping::ProbabilityToValue(probability); *mutable_value(index) = ProbabilityToValue(probability);
} }
// Finishes the update sequence. // Finishes the update sequence.
void FinishUpdate() { void FinishUpdate() {
while (!update_indices_.empty()) { while (!update_indices_.empty()) {
DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker); DCHECK_GE(*update_indices_.back(), kUpdateMarker);
*update_indices_.back() -= mapping::kUpdateMarker; *update_indices_.back() -= kUpdateMarker;
update_indices_.pop_back(); update_indices_.pop_back();
} }
} }
@ -502,20 +502,20 @@ class HybridGrid : public HybridGridBase<uint16> {
// will be set to probability corresponding to 'odds'. // will be set to probability corresponding to 'odds'.
bool ApplyLookupTable(const Eigen::Array3i& index, bool ApplyLookupTable(const Eigen::Array3i& index,
const std::vector<uint16>& table) { const std::vector<uint16>& table) {
DCHECK_EQ(table.size(), mapping::kUpdateMarker); DCHECK_EQ(table.size(), kUpdateMarker);
uint16* const cell = mutable_value(index); uint16* const cell = mutable_value(index);
if (*cell >= mapping::kUpdateMarker) { if (*cell >= kUpdateMarker) {
return false; return false;
} }
update_indices_.push_back(cell); update_indices_.push_back(cell);
*cell = table[*cell]; *cell = table[*cell];
DCHECK_GE(*cell, mapping::kUpdateMarker); DCHECK_GE(*cell, kUpdateMarker);
return true; return true;
} }
// Returns the probability of the cell with 'index'. // Returns the probability of the cell with 'index'.
float GetProbability(const Eigen::Array3i& index) const { float GetProbability(const Eigen::Array3i& index) const {
return mapping::ValueToProbability(value(index)); return ValueToProbability(value(index));
} }
// Returns true if the probability at the specified 'index' is known. // Returns true if the probability at the specified 'index' is known.
@ -540,7 +540,7 @@ class HybridGrid : public HybridGridBase<uint16> {
std::vector<ValueType*> update_indices_; std::vector<ValueType*> update_indices_;
}; };
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ #endif // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_

View File

@ -23,7 +23,7 @@
#include "gmock/gmock.h" #include "gmock/gmock.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
namespace { namespace {
TEST(HybridGridTest, ApplyOdds) { TEST(HybridGridTest, ApplyOdds) {
@ -40,35 +40,30 @@ TEST(HybridGridTest, ApplyOdds) {
hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f); hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f);
hybrid_grid.ApplyLookupTable( hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 0, 1),
Eigen::Array3i(1, 0, 1), ComputeLookupTableToApplyOdds(Odds(0.9f)));
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
hybrid_grid.FinishUpdate(); hybrid_grid.FinishUpdate();
EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f); EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f);
hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f); hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f);
hybrid_grid.ApplyLookupTable( hybrid_grid.ApplyLookupTable(Eigen::Array3i(0, 1, 0),
Eigen::Array3i(0, 1, 0), ComputeLookupTableToApplyOdds(Odds(0.1f)));
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1f)));
hybrid_grid.FinishUpdate(); hybrid_grid.FinishUpdate();
EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f); EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f);
// Tests adding odds to an unknown cell. // Tests adding odds to an unknown cell.
hybrid_grid.ApplyLookupTable( hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1),
Eigen::Array3i(1, 1, 1), ComputeLookupTableToApplyOdds(Odds(0.42f)));
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42f)));
EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
// Tests that further updates are ignored if FinishUpdate() isn't called. // Tests that further updates are ignored if FinishUpdate() isn't called.
hybrid_grid.ApplyLookupTable( hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1),
Eigen::Array3i(1, 1, 1), ComputeLookupTableToApplyOdds(Odds(0.9f)));
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
hybrid_grid.FinishUpdate(); hybrid_grid.FinishUpdate();
hybrid_grid.ApplyLookupTable( hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1),
Eigen::Array3i(1, 1, 1), ComputeLookupTableToApplyOdds(Odds(0.9f)));
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f); EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f);
} }
@ -77,10 +72,10 @@ TEST(HybridGridTest, GetProbability) {
hybrid_grid.SetProbability( hybrid_grid.SetProbability(
hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)), hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)),
mapping::kMaxProbability); kMaxProbability);
EXPECT_NEAR(hybrid_grid.GetProbability( EXPECT_NEAR(hybrid_grid.GetProbability(
hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))), hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))),
mapping::kMaxProbability, 1e-6); kMaxProbability, 1e-6);
for (const Eigen::Array3i& index : for (const Eigen::Array3i& index :
{hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)), {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)),
hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)), hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)),
@ -129,8 +124,8 @@ class RandomHybridGridTest : public ::testing::Test {
public: public:
RandomHybridGridTest() : hybrid_grid_(2.f), values_() { RandomHybridGridTest() : hybrid_grid_(2.f), values_() {
std::mt19937 rng(1285120005); std::mt19937 rng(1285120005);
std::uniform_real_distribution<float> value_distribution( std::uniform_real_distribution<float> value_distribution(kMinProbability,
mapping::kMinProbability, mapping::kMaxProbability); kMaxProbability);
std::uniform_int_distribution<int> xyz_distribution(-3000, 2999); std::uniform_int_distribution<int> xyz_distribution(-3000, 2999);
for (int i = 0; i < 10000; ++i) { for (int i = 0; i < 10000; ++i) {
const auto x = xyz_distribution(rng); const auto x = xyz_distribution(rng);
@ -156,8 +151,7 @@ class RandomHybridGridTest : public ::testing::Test {
TEST_F(RandomHybridGridTest, TestIteration) { TEST_F(RandomHybridGridTest, TestIteration) {
for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) { for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) {
const Eigen::Array3i cell_index = it.GetCellIndex(); const Eigen::Array3i cell_index = it.GetCellIndex();
const float iterator_probability = const float iterator_probability = ValueToProbability(it.GetValue());
mapping::ValueToProbability(it.GetValue());
EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index)); EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index));
const std::tuple<int, int, int> key = const std::tuple<int, int, int> key =
std::make_tuple(cell_index[0], cell_index[1], cell_index[2]); std::make_tuple(cell_index[0], cell_index[1], cell_index[2]);
@ -228,5 +222,5 @@ TEST_F(RandomHybridGridTest, FromProto) {
} }
} // namespace } // namespace
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -14,21 +14,21 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_3d/local_trajectory_builder_options.h" #include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h"
#include "cartographer/internal/mapping/motion_filter.h" #include "cartographer/internal/mapping/motion_filter.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/voxel_filter.h" #include "cartographer/sensor/voxel_filter.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions options; proto::LocalTrajectoryBuilderOptions3D options;
options.set_min_range(parameter_dictionary->GetDouble("min_range")); options.set_min_range(parameter_dictionary->GetDouble("min_range"));
options.set_max_range(parameter_dictionary->GetDouble("max_range")); options.set_max_range(parameter_dictionary->GetDouble("max_range"));
options.set_num_accumulated_range_data( options.set_num_accumulated_range_data(
@ -53,18 +53,18 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
->GetDictionary("real_time_correlative_scan_matcher") ->GetDictionary("real_time_correlative_scan_matcher")
.get()); .get());
*options.mutable_ceres_scan_matcher_options() = *options.mutable_ceres_scan_matcher_options() =
scan_matching::CreateCeresScanMatcherOptions( mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions( *options.mutable_motion_filter_options() = CreateMotionFilterOptions(
parameter_dictionary->GetDictionary("motion_filter").get()); parameter_dictionary->GetDictionary("motion_filter").get());
options.set_imu_gravity_time_constant( options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant")); parameter_dictionary->GetDouble("imu_gravity_time_constant"));
options.set_rotational_histogram_size( options.set_rotational_histogram_size(
parameter_dictionary->GetInt("rotational_histogram_size")); parameter_dictionary->GetInt("rotational_histogram_size"));
*options.mutable_submaps_options() = mapping_3d::CreateSubmapsOptions( *options.mutable_submaps_options() = CreateSubmapsOptions3D(
parameter_dictionary->GetDictionary("submaps").get()); parameter_dictionary->GetDictionary("submaps").get());
return options; return options;
} }
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -14,19 +14,19 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_
#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_

View File

@ -55,7 +55,7 @@ ConstraintBuilder::~ConstraintBuilder() {
} }
void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap,
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data, const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
@ -82,7 +82,7 @@ void ConstraintBuilder::MaybeAddConstraint(
} }
void ConstraintBuilder::MaybeAddGlobalConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap,
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data, const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
@ -123,7 +123,8 @@ void ConstraintBuilder::WhenDone(
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id, const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Submap* const submap, const std::function<void()>& work_item) { const mapping::Submap3D* const submap,
const std::function<void()>& work_item) {
if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
nullptr) { nullptr) {
thread_pool_->Schedule(work_item); thread_pool_->Schedule(work_item);
@ -140,7 +141,7 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
void ConstraintBuilder::ConstructSubmapScanMatcher( void ConstraintBuilder::ConstructSubmapScanMatcher(
const mapping::SubmapId& submap_id, const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Submap* const submap) { const mapping::Submap3D* const submap) {
auto submap_scan_matcher = auto submap_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher>( common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
submap->high_resolution_hybrid_grid(), submap->high_resolution_hybrid_grid(),

View File

@ -36,7 +36,7 @@
#include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.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_3d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/voxel_filter.h" #include "cartographer/sensor/voxel_filter.h"
@ -75,7 +75,7 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished. // all computations are finished.
void MaybeAddConstraint( void MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, const mapping::SubmapId& submap_id, const mapping::Submap3D* submap,
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data, const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
@ -92,7 +92,7 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished. // all computations are finished.
void MaybeAddGlobalConstraint( void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, const mapping::SubmapId& submap_id, const mapping::Submap3D* submap,
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data, const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
@ -114,8 +114,8 @@ class ConstraintBuilder {
private: private:
struct SubmapScanMatcher { struct SubmapScanMatcher {
const HybridGrid* high_resolution_hybrid_grid; const mapping::HybridGrid* high_resolution_hybrid_grid;
const HybridGrid* low_resolution_hybrid_grid; const mapping::HybridGrid* low_resolution_hybrid_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher> std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
fast_correlative_scan_matcher; fast_correlative_scan_matcher;
}; };
@ -125,14 +125,14 @@ class ConstraintBuilder {
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id, const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Submap* submap, const std::function<void()>& work_item) const mapping::Submap3D* submap, const std::function<void()>& work_item)
REQUIRES(mutex_); REQUIRES(mutex_);
// Constructs the scan matcher for a 'submap', then schedules its work items. // Constructs the scan matcher for a 'submap', then schedules its work items.
void ConstructSubmapScanMatcher( void ConstructSubmapScanMatcher(
const mapping::SubmapId& submap_id, const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Submap* submap) EXCLUDES(mutex_); const mapping::Submap3D* submap) EXCLUDES(mutex_);
// Returns the scan matcher for a submap, which has to exist. // Returns the scan matcher for a submap, which has to exist.
const SubmapScanMatcher* GetSubmapScanMatcher( const SubmapScanMatcher* GetSubmapScanMatcher(

View File

@ -279,7 +279,7 @@ void OptimizationProblem::Solve(
CeresPose(submap_id_data.data.global_pose, CeresPose(submap_id_data.data.global_pose,
translation_parameterization(), translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization< common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(), mapping::ConstantYawQuaternionPlus, 4, 2>>(),
&problem)); &problem));
problem.SetParameterBlockConstant( problem.SetParameterBlockConstant(
C_submaps.at(submap_id_data.id).translation()); C_submaps.at(submap_id_data.id).translation());
@ -499,7 +499,7 @@ void OptimizationProblem::Solve(
Eigen::Vector3d::UnitZ())), Eigen::Vector3d::UnitZ())),
nullptr, nullptr,
common::make_unique<ceres::AutoDiffLocalParameterization< common::make_unique<ceres::AutoDiffLocalParameterization<
YawOnlyQuaternionPlus, 4, 1>>(), mapping::YawOnlyQuaternionPlus, 4, 1>>(),
&problem)); &problem));
fixed_frame_pose_initialized = true; fixed_frame_pose_initialized = true;
} }

View File

@ -55,8 +55,7 @@ PoseGraph3D::~PoseGraph3D() {
std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses( std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const mapping_3d::Submap>>& const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) {
insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
@ -102,8 +101,7 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
NodeId PoseGraph3D::AddNode( NodeId PoseGraph3D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data, std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id, const int trajectory_id,
const std::vector<std::shared_ptr<const mapping_3d::Submap>>& const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) {
insertion_submaps) {
const transform::Rigid3d optimized_pose( const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
@ -255,7 +253,7 @@ void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
void PoseGraph3D::ComputeConstraintsForNode( void PoseGraph3D::ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses( const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
@ -446,8 +444,8 @@ void PoseGraph3D::AddSubmapFromProto(
const SubmapId submap_id = {submap.submap_id().trajectory_id(), const SubmapId submap_id = {submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()}; submap.submap_id().submap_index()};
std::shared_ptr<const mapping_3d::Submap> submap_ptr = std::shared_ptr<const Submap3D> submap_ptr =
std::make_shared<const mapping_3d::Submap>(submap.submap_3d()); std::make_shared<const Submap3D>(submap.submap_3d());
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(submap_id.trajectory_id); AddTrajectoryIfNeeded(submap_id.trajectory_id);
@ -483,7 +481,7 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
} }
void PoseGraph3D::SetTrajectoryDataFromProto( void PoseGraph3D::SetTrajectoryDataFromProto(
const mapping::proto::TrajectoryData& data) { const proto::TrajectoryData& data) {
TrajectoryData trajectory_data; TrajectoryData trajectory_data;
trajectory_data.gravity_constant = data.gravity_constant(); trajectory_data.gravity_constant = data.gravity_constant();
trajectory_data.imu_calibration = { trajectory_data.imu_calibration = {
@ -680,7 +678,7 @@ PoseGraph3D::GetFixedFramePoseData() {
return optimization_problem_.fixed_frame_pose_data(); return optimization_problem_.fixed_frame_pose_data();
} }
std::map<int, mapping::PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph3D::GetTrajectoryData() { PoseGraph3D::GetTrajectoryData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.trajectory_data(); return optimization_problem_.trajectory_data();

View File

@ -37,7 +37,7 @@
#include "cartographer/mapping/trajectory_connectivity_state.h" #include "cartographer/mapping/trajectory_connectivity_state.h"
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h" #include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h" #include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -70,10 +70,11 @@ class PoseGraph3D : public PoseGraph {
// node data was inserted into the 'insertion_submaps'. If // node data was inserted into the 'insertion_submaps'. If
// 'insertion_submaps.front().finished()' is 'true', data was inserted into // 'insertion_submaps.front().finished()' is 'true', data was inserted into
// this submap for the last time. // this submap for the last time.
NodeId AddNode(std::shared_ptr<const TrajectoryNode::Data> constant_data, NodeId AddNode(
int trajectory_id, std::shared_ptr<const TrajectoryNode::Data> constant_data,
const std::vector<std::shared_ptr<const mapping_3d::Submap>>& int trajectory_id,
insertion_submaps) EXCLUDES(mutex_); const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
EXCLUDES(mutex_);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_); EXCLUDES(mutex_);
@ -94,10 +95,10 @@ class PoseGraph3D : public PoseGraph {
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override; const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override; const proto::Node& node) override;
void SetTrajectoryDataFromProto(const proto::TrajectoryData& data) override; void SetTrajectoryDataFromProto(const proto::TrajectoryData& data) override;
void AddNodeToSubmap(const mapping::NodeId& node_id, void AddNodeToSubmap(const NodeId& node_id,
const mapping::SubmapId& submap_id) override; const SubmapId& submap_id) override;
void AddSerializedConstraints( void AddSerializedConstraints(
const std::vector<Constraint>& constraints) override; const std::vector<Constraint>& constraints) override;
void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override; void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
@ -137,7 +138,7 @@ class PoseGraph3D : public PoseGraph {
// Likewise, all new nodes are matched against submaps which are finished. // Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished }; enum class SubmapState { kActive, kFinished };
struct SubmapData { struct SubmapData {
std::shared_ptr<const mapping_3d::Submap> submap; std::shared_ptr<const Submap3D> submap;
// IDs of the nodes that were inserted into this map together with // 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 // constraints for them. They are not to be matched again when this submap
@ -157,13 +158,13 @@ class PoseGraph3D : public PoseGraph {
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<SubmapId> InitializeGlobalSubmapPoses( std::vector<SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time, int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const mapping_3d::Submap>>& const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
insertion_submaps) REQUIRES(mutex_); REQUIRES(mutex_);
// Adds constraints for a node, and starts scan matching in the background. // Adds constraints for a node, and starts scan matching in the background.
void ComputeConstraintsForNode( void ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
bool newly_finished_submap) REQUIRES(mutex_); bool newly_finished_submap) REQUIRES(mutex_);
// Computes constraints for a node and submap pair. // Computes constraints for a node and submap pair.

View File

@ -14,7 +14,7 @@
syntax = "proto3"; syntax = "proto3";
package cartographer.mapping_3d.proto; package cartographer.mapping.proto;
message HybridGrid { message HybridGrid {
float resolution = 1; float resolution = 1;

View File

@ -14,16 +14,16 @@
syntax = "proto3"; syntax = "proto3";
package cartographer.mapping_3d.proto; package cartographer.mapping.proto;
import "cartographer/mapping/proto/motion_filter_options.proto"; import "cartographer/mapping/proto/motion_filter_options.proto";
import "cartographer/sensor/proto/adaptive_voxel_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_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
import "cartographer/mapping_3d/proto/submaps_options.proto"; import "cartographer/mapping_3d/proto/submaps_options_3d.proto";
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
// NEXT ID: 18 // NEXT ID: 18
message LocalTrajectoryBuilderOptions { message LocalTrajectoryBuilderOptions3D {
// Rangefinder points outside these ranges will be dropped. // Rangefinder points outside these ranges will be dropped.
float min_range = 1; float min_range = 1;
float max_range = 2; float max_range = 2;
@ -47,7 +47,7 @@ message LocalTrajectoryBuilderOptions {
bool use_online_correlative_scan_matching = 13; bool use_online_correlative_scan_matching = 13;
mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 14; real_time_correlative_scan_matcher_options = 14;
scan_matching.proto.CeresScanMatcherOptions mapping_3d.scan_matching.proto.CeresScanMatcherOptions
ceres_scan_matcher_options = 6; ceres_scan_matcher_options = 6;
mapping.proto.MotionFilterOptions motion_filter_options = 7; mapping.proto.MotionFilterOptions motion_filter_options = 7;
@ -62,5 +62,5 @@ message LocalTrajectoryBuilderOptions {
// Number of histogram buckets for the rotational scan matcher. // Number of histogram buckets for the rotational scan matcher.
int32 rotational_histogram_size = 17; int32 rotational_histogram_size = 17;
SubmapsOptions submaps_options = 8; SubmapsOptions3D submaps_options = 8;
} }

View File

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

View File

@ -14,11 +14,11 @@
syntax = "proto3"; syntax = "proto3";
import "cartographer/mapping_3d/proto/range_data_inserter_options.proto"; import "cartographer/mapping_3d/proto/range_data_inserter_options_3d.proto";
package cartographer.mapping_3d.proto; package cartographer.mapping.proto;
message SubmapsOptions { message SubmapsOptions3D {
// Resolution of the 'high_resolution' map in meters used for local SLAM and // Resolution of the 'high_resolution' map in meters used for local SLAM and
// loop closure. // loop closure.
double high_resolution = 1; double high_resolution = 1;
@ -36,5 +36,5 @@ message SubmapsOptions {
// matched against, then while being matched. // matched against, then while being matched.
int32 num_range_data = 2; int32 num_range_data = 2;
RangeDataInserterOptions range_data_inserter_options = 3; RangeDataInserterOptions3D range_data_inserter_options = 3;
} }

View File

@ -14,15 +14,14 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/mapping_3d/range_data_inserter_3d.h"
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping/probability_values.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
namespace { namespace {
void InsertMissesIntoGrid(const std::vector<uint16>& miss_table, void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
@ -54,9 +53,9 @@ void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
} // namespace } // namespace
proto::RangeDataInserterOptions CreateRangeDataInserterOptions( proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D(
common::LuaParameterDictionary* parameter_dictionary) { common::LuaParameterDictionary* parameter_dictionary) {
proto::RangeDataInserterOptions options; proto::RangeDataInserterOptions3D options;
options.set_hit_probability( options.set_hit_probability(
parameter_dictionary->GetDouble("hit_probability")); parameter_dictionary->GetDouble("hit_probability"));
options.set_miss_probability( options.set_miss_probability(
@ -68,16 +67,16 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
return options; return options;
} }
RangeDataInserter::RangeDataInserter( RangeDataInserter3D::RangeDataInserter3D(
const proto::RangeDataInserterOptions& options) const proto::RangeDataInserterOptions3D& options)
: options_(options), : options_(options),
hit_table_(mapping::ComputeLookupTableToApplyOdds( hit_table_(
mapping::Odds(options_.hit_probability()))), ComputeLookupTableToApplyOdds(Odds(options_.hit_probability()))),
miss_table_(mapping::ComputeLookupTableToApplyOdds( miss_table_(
mapping::Odds(options_.miss_probability()))) {} ComputeLookupTableToApplyOdds(Odds(options_.miss_probability()))) {}
void RangeDataInserter::Insert(const sensor::RangeData& range_data, void RangeDataInserter3D::Insert(const sensor::RangeData& range_data,
HybridGrid* hybrid_grid) const { HybridGrid* hybrid_grid) const {
CHECK_NOTNULL(hybrid_grid); CHECK_NOTNULL(hybrid_grid);
for (const Eigen::Vector3f& hit : range_data.returns) { for (const Eigen::Vector3f& hit : range_data.returns) {
@ -92,5 +91,5 @@ void RangeDataInserter::Insert(const sensor::RangeData& range_data,
hybrid_grid->FinishUpdate(); hybrid_grid->FinishUpdate();
} }
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -14,38 +14,39 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ #ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_
#define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ #define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h" #include "cartographer/mapping_3d/proto/range_data_inserter_options_3d.pb.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
proto::RangeDataInserterOptions CreateRangeDataInserterOptions( proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
class RangeDataInserter { class RangeDataInserter3D {
public: public:
explicit RangeDataInserter(const proto::RangeDataInserterOptions& options); explicit RangeDataInserter3D(
const proto::RangeDataInserterOptions3D& options);
RangeDataInserter(const RangeDataInserter&) = delete; RangeDataInserter3D(const RangeDataInserter3D&) = delete;
RangeDataInserter& operator=(const RangeDataInserter&) = delete; RangeDataInserter3D& operator=(const RangeDataInserter3D&) = delete;
// Inserts 'range_data' into 'hybrid_grid'. // Inserts 'range_data' into 'hybrid_grid'.
void Insert(const sensor::RangeData& range_data, void Insert(const sensor::RangeData& range_data,
HybridGrid* hybrid_grid) const; HybridGrid* hybrid_grid) const;
private: private:
const proto::RangeDataInserterOptions options_; const proto::RangeDataInserterOptions3D options_;
const std::vector<uint16> hit_table_; const std::vector<uint16> hit_table_;
const std::vector<uint16> miss_table_; const std::vector<uint16> miss_table_;
}; };
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ #endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_

View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/mapping_3d/range_data_inserter_3d.h"
#include <memory> #include <memory>
#include <vector> #include <vector>
@ -23,20 +23,20 @@
#include "gmock/gmock.h" #include "gmock/gmock.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
namespace { namespace {
class RangeDataInserterTest : public ::testing::Test { class RangeDataInserter3DTest : public ::testing::Test {
protected: protected:
RangeDataInserterTest() : hybrid_grid_(1.f) { RangeDataInserter3DTest() : hybrid_grid_(1.f) {
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
"return { " "return { "
"hit_probability = 0.7, " "hit_probability = 0.7, "
"miss_probability = 0.4, " "miss_probability = 0.4, "
"num_free_space_voxels = 1000, " "num_free_space_voxels = 1000, "
"}"); "}");
options_ = CreateRangeDataInserterOptions(parameter_dictionary.get()); options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get());
range_data_inserter_.reset(new RangeDataInserter(options_)); range_data_inserter_.reset(new RangeDataInserter3D(options_));
} }
void InsertPointCloud() { void InsertPointCloud() {
@ -57,15 +57,15 @@ class RangeDataInserterTest : public ::testing::Test {
hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
} }
const proto::RangeDataInserterOptions& options() const { return options_; } const proto::RangeDataInserterOptions3D& options() const { return options_; }
private: private:
HybridGrid hybrid_grid_; HybridGrid hybrid_grid_;
std::unique_ptr<RangeDataInserter> range_data_inserter_; std::unique_ptr<RangeDataInserter3D> range_data_inserter_;
proto::RangeDataInserterOptions options_; proto::RangeDataInserterOptions3D options_;
}; };
TEST_F(RangeDataInserterTest, InsertPointCloud) { TEST_F(RangeDataInserter3DTest, InsertPointCloud) {
InsertPointCloud(); InsertPointCloud();
EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f), EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f),
1e-4); 1e-4);
@ -85,7 +85,7 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) {
} }
} }
TEST_F(RangeDataInserterTest, ProbabilityProgression) { TEST_F(RangeDataInserter3DTest, ProbabilityProgression) {
InsertPointCloud(); InsertPointCloud();
EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f), EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f),
1e-4); 1e-4);
@ -97,11 +97,11 @@ TEST_F(RangeDataInserterTest, ProbabilityProgression) {
for (int i = 0; i < 1000; ++i) { for (int i = 0; i < 1000; ++i) {
InsertPointCloud(); InsertPointCloud();
} }
EXPECT_NEAR(mapping::kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3); EXPECT_NEAR(kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3);
EXPECT_NEAR(mapping::kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3); EXPECT_NEAR(kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3);
EXPECT_NEAR(mapping::kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3); EXPECT_NEAR(kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3);
} }
} // namespace } // namespace
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -22,7 +22,7 @@
#include "ceres/rotation.h" #include "ceres/rotation.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
struct YawOnlyQuaternionPlus { struct YawOnlyQuaternionPlus {
template <typename T> template <typename T>
@ -61,7 +61,7 @@ struct ConstantYawQuaternionPlus {
} }
}; };
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_ROTATION_PARAMETERIZATION_H_ #endif // CARTOGRAPHER_MAPPING_3D_ROTATION_PARAMETERIZATION_H_

View File

@ -80,7 +80,7 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
options_.only_optimize_yaw() options_.only_optimize_yaw()
? std::unique_ptr<ceres::LocalParameterization>( ? std::unique_ptr<ceres::LocalParameterization>(
common::make_unique<ceres::AutoDiffLocalParameterization< common::make_unique<ceres::AutoDiffLocalParameterization<
YawOnlyQuaternionPlus, 4, 1>>()) mapping::YawOnlyQuaternionPlus, 4, 1>>())
: std::unique_ptr<ceres::LocalParameterization>( : std::unique_ptr<ceres::LocalParameterization>(
common::make_unique<ceres::QuaternionParameterization>()), common::make_unique<ceres::QuaternionParameterization>()),
&problem); &problem);
@ -91,7 +91,8 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
CHECK_GT(options_.occupied_space_weight(i), 0.); CHECK_GT(options_.occupied_space_weight(i), 0.);
const sensor::PointCloud& point_cloud = const sensor::PointCloud& point_cloud =
*point_clouds_and_hybrid_grids[i].first; *point_clouds_and_hybrid_grids[i].first;
const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; const mapping::HybridGrid& hybrid_grid =
*point_clouds_and_hybrid_grids[i].second;
problem.AddResidualBlock( problem.AddResidualBlock(
OccupiedSpaceCostFunction::CreateAutoDiffCostFunction( OccupiedSpaceCostFunction::CreateAutoDiffCostFunction(
options_.occupied_space_weight(i) / options_.occupied_space_weight(i) /

View File

@ -35,7 +35,7 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
using PointCloudAndHybridGridPointers = using PointCloudAndHybridGridPointers =
std::pair<const sensor::PointCloud*, const HybridGrid*>; std::pair<const sensor::PointCloud*, const mapping::HybridGrid*>;
// This scan matcher uses Ceres to align scans with an existing map. // This scan matcher uses Ceres to align scans with an existing map.
class CeresScanMatcher { class CeresScanMatcher {

View File

@ -74,7 +74,7 @@ class CeresScanMatcherTest : public ::testing::Test {
EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
} }
HybridGrid hybrid_grid_; mapping::HybridGrid hybrid_grid_;
transform::Rigid3d expected_pose_; transform::Rigid3d expected_pose_;
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
proto::CeresScanMatcherOptions options_; proto::CeresScanMatcherOptions options_;

View File

@ -58,7 +58,7 @@ CreateFastCorrelativeScanMatcherOptions(
class PrecomputationGridStack { class PrecomputationGridStack {
public: public:
PrecomputationGridStack( PrecomputationGridStack(
const HybridGrid& hybrid_grid, const mapping::HybridGrid& hybrid_grid,
const proto::FastCorrelativeScanMatcherOptions& options) { const proto::FastCorrelativeScanMatcherOptions& options) {
CHECK_GE(options.branch_and_bound_depth(), 1); CHECK_GE(options.branch_and_bound_depth(), 1);
CHECK_GE(options.full_resolution_depth(), 1); CHECK_GE(options.full_resolution_depth(), 1);
@ -142,8 +142,8 @@ std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
} // namespace } // namespace
FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const HybridGrid& hybrid_grid, const mapping::HybridGrid& hybrid_grid,
const HybridGrid* const low_resolution_hybrid_grid, const mapping::HybridGrid* const low_resolution_hybrid_grid,
const std::vector<mapping::TrajectoryNode>& nodes, const std::vector<mapping::TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions& options) const proto::FastCorrelativeScanMatcherOptions& options)
: options_(options), : options_(options),

View File

@ -57,8 +57,8 @@ class FastCorrelativeScanMatcher {
}; };
FastCorrelativeScanMatcher( FastCorrelativeScanMatcher(
const HybridGrid& hybrid_grid, const mapping::HybridGrid& hybrid_grid,
const HybridGrid* low_resolution_hybrid_grid, const mapping::HybridGrid* low_resolution_hybrid_grid,
const std::vector<mapping::TrajectoryNode>& nodes, const std::vector<mapping::TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions& options); const proto::FastCorrelativeScanMatcherOptions& options);
~FastCorrelativeScanMatcher(); ~FastCorrelativeScanMatcher();
@ -132,7 +132,7 @@ class FastCorrelativeScanMatcher {
const float resolution_; const float resolution_;
const int width_in_voxels_; const int width_in_voxels_;
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_; std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
const HybridGrid* const low_resolution_hybrid_grid_; const mapping::HybridGrid* const low_resolution_hybrid_grid_;
RotationalScanMatcher rotational_scan_matcher_; RotationalScanMatcher rotational_scan_matcher_;
}; };

View File

@ -23,7 +23,7 @@
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/mapping_3d/range_data_inserter_3d.h"
#include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/rigid_transform_test_helpers.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
@ -81,7 +81,7 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
} }
static mapping_3d::proto::RangeDataInserterOptions static mapping::proto::RangeDataInserterOptions3D
CreateRangeDataInserterTestOptions() { CreateRangeDataInserterTestOptions() {
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
"return { " "return { "
@ -89,13 +89,14 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
"miss_probability = 0.4, " "miss_probability = 0.4, "
"num_free_space_voxels = 5, " "num_free_space_voxels = 5, "
"}"); "}");
return CreateRangeDataInserterOptions(parameter_dictionary.get()); return mapping::CreateRangeDataInserterOptions3D(
parameter_dictionary.get());
} }
std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher( std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher(
const proto::FastCorrelativeScanMatcherOptions& options, const proto::FastCorrelativeScanMatcherOptions& options,
const transform::Rigid3f& pose) { const transform::Rigid3f& pose) {
hybrid_grid_ = common::make_unique<HybridGrid>(0.05f); hybrid_grid_ = common::make_unique<mapping::HybridGrid>(0.05f);
range_data_inserter_.Insert( range_data_inserter_.Insert(
sensor::RangeData{pose.translation(), sensor::RangeData{pose.translation(),
sensor::TransformPointCloud(point_cloud_, pose), sensor::TransformPointCloud(point_cloud_, pose),
@ -125,10 +126,10 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
std::mt19937 prng_ = std::mt19937(42); std::mt19937 prng_ = std::mt19937(42);
std::uniform_real_distribution<float> distribution_ = std::uniform_real_distribution<float> distribution_ =
std::uniform_real_distribution<float>(-1.f, 1.f); std::uniform_real_distribution<float>(-1.f, 1.f);
RangeDataInserter range_data_inserter_; mapping::RangeDataInserter3D range_data_inserter_;
const proto::FastCorrelativeScanMatcherOptions options_; const proto::FastCorrelativeScanMatcherOptions options_;
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<HybridGrid> hybrid_grid_; std::unique_ptr<mapping::HybridGrid> hybrid_grid_;
}; };
constexpr float kMinScore = 0.1f; constexpr float kMinScore = 0.1f;

View File

@ -34,7 +34,7 @@ namespace scan_matching {
// continuously differentiable. // continuously differentiable.
class InterpolatedGrid { class InterpolatedGrid {
public: public:
explicit InterpolatedGrid(const HybridGrid& hybrid_grid) explicit InterpolatedGrid(const mapping::HybridGrid& hybrid_grid)
: hybrid_grid_(hybrid_grid) {} : hybrid_grid_(hybrid_grid) {}
InterpolatedGrid(const InterpolatedGrid&) = delete; InterpolatedGrid(const InterpolatedGrid&) = delete;
@ -145,7 +145,7 @@ class InterpolatedGrid {
return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
} }
const HybridGrid& hybrid_grid_; const mapping::HybridGrid& hybrid_grid_;
}; };
} // namespace scan_matching } // namespace scan_matching

View File

@ -43,7 +43,7 @@ class InterpolatedGridTest : public ::testing::Test {
hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
} }
HybridGrid hybrid_grid_; mapping::HybridGrid hybrid_grid_;
InterpolatedGrid interpolated_grid_; InterpolatedGrid interpolated_grid_;
}; };

View File

@ -21,7 +21,8 @@ namespace mapping_3d {
namespace scan_matching { namespace scan_matching {
std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher( std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) { const mapping::HybridGrid* low_resolution_grid,
const sensor::PointCloud* points) {
return [=](const transform::Rigid3f& pose) { return [=](const transform::Rigid3f& pose) {
float score = 0.f; float score = 0.f;
for (const Eigen::Vector3f& point : for (const Eigen::Vector3f& point :

View File

@ -28,7 +28,8 @@ namespace mapping_3d {
namespace scan_matching { namespace scan_matching {
std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher( std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points); const mapping::HybridGrid* low_resolution_grid,
const sensor::PointCloud* points);
} // namespace scan_matching } // namespace scan_matching
} // namespace mapping_3d } // namespace mapping_3d

View File

@ -47,9 +47,11 @@ Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) {
} // namespace } // namespace
PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) { PrecomputationGrid ConvertToPrecomputationGrid(
const mapping::HybridGrid& hybrid_grid) {
PrecomputationGrid result(hybrid_grid.resolution()); PrecomputationGrid result(hybrid_grid.resolution());
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { for (auto it = mapping::HybridGrid::Iterator(hybrid_grid); !it.Done();
it.Next()) {
const int cell_value = common::RoundToInt( const int cell_value = common::RoundToInt(
(mapping::ValueToProbability(it.GetValue()) - (mapping::ValueToProbability(it.GetValue()) -
mapping::kMinProbability) * mapping::kMinProbability) *

View File

@ -23,10 +23,10 @@ namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
namespace scan_matching { namespace scan_matching {
class PrecomputationGrid : public HybridGridBase<uint8> { class PrecomputationGrid : public mapping::HybridGridBase<uint8> {
public: public:
explicit PrecomputationGrid(const float resolution) explicit PrecomputationGrid(const float resolution)
: HybridGridBase<uint8>(resolution) {} : mapping::HybridGridBase<uint8>(resolution) {}
// Maps values from [0, 255] to [kMinProbability, kMaxProbability]. // Maps values from [0, 255] to [kMinProbability, kMaxProbability].
static float ToProbability(float value) { static float ToProbability(float value) {
@ -38,7 +38,8 @@ class PrecomputationGrid : public HybridGridBase<uint8> {
// Converts a HybridGrid to a PrecomputationGrid representing the same data, // Converts a HybridGrid to a PrecomputationGrid representing the same data,
// but only using 8 bit instead of 2 x 16 bit. // but only using 8 bit instead of 2 x 16 bit.
PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid); PrecomputationGrid ConvertToPrecomputationGrid(
const mapping::HybridGrid& hybrid_grid);
// Returns a grid of the same resolution containing the maximum value of // Returns a grid of the same resolution containing the maximum value of
// original voxels in 'grid'. This maximum is over the 8 voxels that have // original voxels in 'grid'. This maximum is over the 8 voxels that have

View File

@ -29,7 +29,7 @@ namespace scan_matching {
namespace { namespace {
TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
HybridGrid hybrid_grid(2.f); mapping::HybridGrid hybrid_grid(2.f);
std::mt19937 rng(23847); std::mt19937 rng(23847);
std::uniform_int_distribution<int> coordinate_distribution(-50, 49); std::uniform_int_distribution<int> coordinate_distribution(-50, 49);

View File

@ -34,7 +34,8 @@ RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
float RealTimeCorrelativeScanMatcher::Match( float RealTimeCorrelativeScanMatcher::Match(
const transform::Rigid3d& initial_pose_estimate, const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid, const sensor::PointCloud& point_cloud,
const mapping::HybridGrid& hybrid_grid,
transform::Rigid3d* pose_estimate) const { transform::Rigid3d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate); CHECK_NOTNULL(pose_estimate);
float best_score = -1.f; float best_score = -1.f;
@ -96,7 +97,7 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
} }
float RealTimeCorrelativeScanMatcher::ScoreCandidate( float RealTimeCorrelativeScanMatcher::ScoreCandidate(
const HybridGrid& hybrid_grid, const mapping::HybridGrid& hybrid_grid,
const sensor::PointCloud& transformed_point_cloud, const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const { const transform::Rigid3f& transform) const {
float score = 0.f; float score = 0.f;

View File

@ -46,13 +46,13 @@ class RealTimeCorrelativeScanMatcher {
// returns the score. // returns the score.
float Match(const transform::Rigid3d& initial_pose_estimate, float Match(const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const sensor::PointCloud& point_cloud,
const HybridGrid& hybrid_grid, const mapping::HybridGrid& hybrid_grid,
transform::Rigid3d* pose_estimate) const; transform::Rigid3d* pose_estimate) const;
private: private:
std::vector<transform::Rigid3f> GenerateExhaustiveSearchTransforms( std::vector<transform::Rigid3f> GenerateExhaustiveSearchTransforms(
float resolution, const sensor::PointCloud& point_cloud) const; float resolution, const sensor::PointCloud& point_cloud) const;
float ScoreCandidate(const HybridGrid& hybrid_grid, float ScoreCandidate(const mapping::HybridGrid& hybrid_grid,
const sensor::PointCloud& transformed_point_cloud, const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const; const transform::Rigid3f& transform) const;

View File

@ -72,7 +72,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3)); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3));
} }
HybridGrid hybrid_grid_; mapping::HybridGrid hybrid_grid_;
transform::Rigid3d expected_pose_; transform::Rigid3d expected_pose_;
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher> std::unique_ptr<RealTimeCorrelativeScanMatcher>

View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submap_3d.h"
#include <cmath> #include <cmath>
#include <limits> #include <limits>
@ -24,8 +24,7 @@
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
namespace { namespace {
struct PixelData { struct PixelData {
@ -69,7 +68,7 @@ std::vector<PixelData> AccumulatePixelData(
pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]); pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]);
pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]); pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]);
const float probability = const float probability =
mapping::ValueToProbability(voxel_index_and_probability[3]); ValueToProbability(voxel_index_and_probability[3]);
pixel.probability_sum += probability; pixel.probability_sum += probability;
pixel.max_probability = std::max(pixel.max_probability, probability); pixel.max_probability = std::max(pixel.max_probability, probability);
} }
@ -88,7 +87,7 @@ std::vector<Eigen::Array4i> ExtractVoxelData(
constexpr float kXrayObstructedCellProbabilityLimit = 0.501f; constexpr float kXrayObstructedCellProbabilityLimit = 0.501f;
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
const uint16 probability_value = it.GetValue(); const uint16 probability_value = it.GetValue();
const float probability = mapping::ValueToProbability(probability_value); const float probability = ValueToProbability(probability_value);
if (probability < kXrayObstructedCellProbabilityLimit) { if (probability < kXrayObstructedCellProbabilityLimit) {
// We ignore non-obstructed cells. // We ignore non-obstructed cells.
continue; continue;
@ -133,11 +132,10 @@ std::string ComputePixelValues(
const float free_space_weight = kFreeSpaceWeight * free_space; const float free_space_weight = kFreeSpaceWeight * free_space;
const float total_weight = pixel.count + free_space_weight; const float total_weight = pixel.count + free_space_weight;
const float free_space_probability = 1.f - pixel.max_probability; const float free_space_probability = 1.f - pixel.max_probability;
const float average_probability = mapping::ClampProbability( const float average_probability = ClampProbability(
(pixel.probability_sum + free_space_probability * free_space_weight) / (pixel.probability_sum + free_space_probability * free_space_weight) /
total_weight); total_weight);
const int delta = const int delta = 128 - ProbabilityToLogOddsInteger(average_probability);
128 - mapping::ProbabilityToLogOddsInteger(average_probability);
const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0; const uint8 value = delta > 0 ? delta : 0;
cell_data.push_back(value); // value cell_data.push_back(value); // value
@ -148,7 +146,7 @@ std::string ComputePixelValues(
void AddToTextureProto( void AddToTextureProto(
const HybridGrid& hybrid_grid, const transform::Rigid3d& global_submap_pose, const HybridGrid& hybrid_grid, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response::SubmapTexture* const texture) { proto::SubmapQuery::Response::SubmapTexture* const texture) {
// Generate an X-ray view through the 'hybrid_grid', aligned to the // Generate an X-ray view through the 'hybrid_grid', aligned to the
// xy-plane in the global map frame. // xy-plane in the global map frame.
const float resolution = hybrid_grid.resolution(); const float resolution = hybrid_grid.resolution();
@ -180,9 +178,9 @@ void AddToTextureProto(
} // namespace } // namespace
proto::SubmapsOptions CreateSubmapsOptions( proto::SubmapsOptions3D CreateSubmapsOptions3D(
common::LuaParameterDictionary* parameter_dictionary) { common::LuaParameterDictionary* parameter_dictionary) {
proto::SubmapsOptions options; proto::SubmapsOptions3D options;
options.set_high_resolution( options.set_high_resolution(
parameter_dictionary->GetDouble("high_resolution")); parameter_dictionary->GetDouble("high_resolution"));
options.set_high_resolution_max_range( options.set_high_resolution_max_range(
@ -191,22 +189,22 @@ proto::SubmapsOptions CreateSubmapsOptions(
options.set_num_range_data( options.set_num_range_data(
parameter_dictionary->GetNonNegativeInt("num_range_data")); parameter_dictionary->GetNonNegativeInt("num_range_data"));
*options.mutable_range_data_inserter_options() = *options.mutable_range_data_inserter_options() =
CreateRangeDataInserterOptions( CreateRangeDataInserterOptions3D(
parameter_dictionary->GetDictionary("range_data_inserter").get()); parameter_dictionary->GetDictionary("range_data_inserter").get());
CHECK_GT(options.num_range_data(), 0); CHECK_GT(options.num_range_data(), 0);
return options; return options;
} }
Submap::Submap(const float high_resolution, const float low_resolution, Submap3D::Submap3D(const float high_resolution, const float low_resolution,
const transform::Rigid3d& local_submap_pose) const transform::Rigid3d& local_submap_pose)
: mapping::Submap(local_submap_pose), : Submap(local_submap_pose),
high_resolution_hybrid_grid_( high_resolution_hybrid_grid_(
common::make_unique<HybridGrid>(high_resolution)), common::make_unique<HybridGrid>(high_resolution)),
low_resolution_hybrid_grid_( low_resolution_hybrid_grid_(
common::make_unique<HybridGrid>(low_resolution)) {} common::make_unique<HybridGrid>(low_resolution)) {}
Submap::Submap(const mapping::proto::Submap3D& proto) Submap3D::Submap3D(const proto::Submap3D& proto)
: mapping::Submap(transform::ToRigid3(proto.local_pose())), : Submap(transform::ToRigid3(proto.local_pose())),
high_resolution_hybrid_grid_( high_resolution_hybrid_grid_(
common::make_unique<HybridGrid>(proto.high_resolution_hybrid_grid())), common::make_unique<HybridGrid>(proto.high_resolution_hybrid_grid())),
low_resolution_hybrid_grid_( low_resolution_hybrid_grid_(
@ -215,8 +213,8 @@ Submap::Submap(const mapping::proto::Submap3D& proto)
SetFinished(proto.finished()); SetFinished(proto.finished());
} }
void Submap::ToProto(mapping::proto::Submap* const proto, void Submap3D::ToProto(proto::Submap* const proto,
bool include_probability_grid_data) const { bool include_probability_grid_data) const {
auto* const submap_3d = proto->mutable_submap_3d(); auto* const submap_3d = proto->mutable_submap_3d();
*submap_3d->mutable_local_pose() = transform::ToProto(local_pose()); *submap_3d->mutable_local_pose() = transform::ToProto(local_pose());
submap_3d->set_num_range_data(num_range_data()); submap_3d->set_num_range_data(num_range_data());
@ -229,7 +227,7 @@ void Submap::ToProto(mapping::proto::Submap* const proto,
} }
} }
void Submap::UpdateFromProto(const mapping::proto::Submap& proto) { void Submap3D::UpdateFromProto(const proto::Submap& proto) {
CHECK(proto.has_submap_3d()); CHECK(proto.has_submap_3d());
const auto& submap_3d = proto.submap_3d(); const auto& submap_3d = proto.submap_3d();
SetNumRangeData(submap_3d.num_range_data()); SetNumRangeData(submap_3d.num_range_data());
@ -250,9 +248,9 @@ void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
} }
} }
void Submap::ToResponseProto( void Submap3D::ToResponseProto(
const transform::Rigid3d& global_submap_pose, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* const response) const { proto::SubmapQuery::Response* const response) const {
response->set_submap_version(num_range_data()); response->set_submap_version(num_range_data());
AddToTextureProto(*high_resolution_hybrid_grid_, global_submap_pose, AddToTextureProto(*high_resolution_hybrid_grid_, global_submap_pose,
@ -261,9 +259,9 @@ void Submap::ToResponseProto(
response->add_textures()); response->add_textures());
} }
void Submap::InsertRangeData(const sensor::RangeData& range_data, void Submap3D::InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter& range_data_inserter, const RangeDataInserter3D& range_data_inserter,
const int high_resolution_max_range) { const int high_resolution_max_range) {
CHECK(!finished()); CHECK(!finished());
const sensor::RangeData transformed_range_data = sensor::TransformRangeData( const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
range_data, local_pose().inverse().cast<float>()); range_data, local_pose().inverse().cast<float>());
@ -276,12 +274,12 @@ void Submap::InsertRangeData(const sensor::RangeData& range_data,
SetNumRangeData(num_range_data() + 1); SetNumRangeData(num_range_data() + 1);
} }
void Submap::Finish() { void Submap3D::Finish() {
CHECK(!finished()); CHECK(!finished());
SetFinished(true); SetFinished(true);
} }
ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options) ActiveSubmaps3D::ActiveSubmaps3D(const proto::SubmapsOptions3D& options)
: options_(options), : options_(options),
range_data_inserter_(options.range_data_inserter_options()) { range_data_inserter_(options.range_data_inserter_options()) {
// We always want to have at least one submap which we can return and will // We always want to have at least one submap which we can return and will
@ -292,13 +290,13 @@ ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
AddSubmap(transform::Rigid3d::Identity()); AddSubmap(transform::Rigid3d::Identity());
} }
std::vector<std::shared_ptr<Submap>> ActiveSubmaps::submaps() const { std::vector<std::shared_ptr<Submap3D>> ActiveSubmaps3D::submaps() const {
return submaps_; return submaps_;
} }
int ActiveSubmaps::matching_index() const { return matching_submap_index_; } int ActiveSubmaps3D::matching_index() const { return matching_submap_index_; }
void ActiveSubmaps::InsertRangeData( void ActiveSubmaps3D::InsertRangeData(
const sensor::RangeData& range_data, const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment) { const Eigen::Quaterniond& gravity_alignment) {
for (auto& submap : submaps_) { for (auto& submap : submaps_) {
@ -311,17 +309,17 @@ void ActiveSubmaps::InsertRangeData(
} }
} }
void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_submap_pose) { void ActiveSubmaps3D::AddSubmap(const transform::Rigid3d& local_submap_pose) {
if (submaps_.size() > 1) { if (submaps_.size() > 1) {
submaps_.front()->Finish(); submaps_.front()->Finish();
++matching_submap_index_; ++matching_submap_index_;
submaps_.erase(submaps_.begin()); submaps_.erase(submaps_.begin());
} }
submaps_.emplace_back(new Submap(options_.high_resolution(), submaps_.emplace_back(new Submap3D(options_.high_resolution(),
options_.low_resolution(), options_.low_resolution(),
local_submap_pose)); local_submap_pose));
LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size(); LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size();
} }
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ #ifndef CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_
#define CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ #define CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_
#include <memory> #include <memory>
#include <string> #include <string>
@ -28,31 +28,30 @@
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/proto/submaps_options.pb.h" #include "cartographer/mapping_3d/proto/submaps_options_3d.pb.h"
#include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/mapping_3d/range_data_inserter_3d.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
proto::SubmapsOptions CreateSubmapsOptions( proto::SubmapsOptions3D CreateSubmapsOptions3D(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
class Submap : public mapping::Submap { class Submap3D : public Submap {
public: public:
Submap(float high_resolution, float low_resolution, Submap3D(float high_resolution, float low_resolution,
const transform::Rigid3d& local_submap_pose); const transform::Rigid3d& local_submap_pose);
explicit Submap(const mapping::proto::Submap3D& proto); explicit Submap3D(const proto::Submap3D& proto);
void ToProto(mapping::proto::Submap* proto, void ToProto(proto::Submap* proto,
bool include_probability_grid_data) const override; bool include_probability_grid_data) const override;
void UpdateFromProto(const mapping::proto::Submap& proto) override; void UpdateFromProto(const proto::Submap& proto) override;
void ToResponseProto( void ToResponseProto(const transform::Rigid3d& global_submap_pose,
const transform::Rigid3d& global_submap_pose, proto::SubmapQuery::Response* response) const override;
mapping::proto::SubmapQuery::Response* response) const override;
const HybridGrid& high_resolution_hybrid_grid() const { const HybridGrid& high_resolution_hybrid_grid() const {
return *high_resolution_hybrid_grid_; return *high_resolution_hybrid_grid_;
@ -64,7 +63,7 @@ class Submap : public mapping::Submap {
// Insert 'range_data' into this submap using 'range_data_inserter'. The // Insert 'range_data' into this submap using 'range_data_inserter'. The
// submap must not be finished yet. // submap must not be finished yet.
void InsertRangeData(const sensor::RangeData& range_data, void InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter& range_data_inserter, const RangeDataInserter3D& range_data_inserter,
int high_resolution_max_range); int high_resolution_max_range);
void Finish(); void Finish();
@ -82,12 +81,12 @@ class Submap : public mapping::Submap {
// considered initialized: the old submap is no longer changed, the "new" 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 // 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. // "new" submap gets created. The "old" submap is forgotten by this object.
class ActiveSubmaps { class ActiveSubmaps3D {
public: public:
explicit ActiveSubmaps(const proto::SubmapsOptions& options); explicit ActiveSubmaps3D(const proto::SubmapsOptions3D& options);
ActiveSubmaps(const ActiveSubmaps&) = delete; ActiveSubmaps3D(const ActiveSubmaps3D&) = delete;
ActiveSubmaps& operator=(const ActiveSubmaps&) = delete; ActiveSubmaps3D& operator=(const ActiveSubmaps3D&) = delete;
// Returns the index of the newest initialized Submap which can be // Returns the index of the newest initialized Submap which can be
// used for scan-to-map matching. // used for scan-to-map matching.
@ -99,18 +98,18 @@ class ActiveSubmaps {
void InsertRangeData(const sensor::RangeData& range_data, void InsertRangeData(const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment); const Eigen::Quaterniond& gravity_alignment);
std::vector<std::shared_ptr<Submap>> submaps() const; std::vector<std::shared_ptr<Submap3D>> submaps() const;
private: private:
void AddSubmap(const transform::Rigid3d& local_submap_pose); void AddSubmap(const transform::Rigid3d& local_submap_pose);
const proto::SubmapsOptions options_; const proto::SubmapsOptions3D options_;
int matching_submap_index_ = 0; int matching_submap_index_ = 0;
std::vector<std::shared_ptr<Submap>> submaps_; std::vector<std::shared_ptr<Submap3D>> submaps_;
RangeDataInserter range_data_inserter_; RangeDataInserter3D range_data_inserter_;
}; };
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ #endif // CARTOGRAPHER_MAPPING_3D_SUBMAP_3D_H_

View File

@ -14,24 +14,25 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "gmock/gmock.h" #include "gmock/gmock.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping {
namespace { namespace {
TEST(SubmapsTest, ToFromProto) { TEST(SubmapsTest, ToFromProto) {
const Submap expected(0.05, 0.25, const Submap3D expected(
transform::Rigid3d(Eigen::Vector3d(1., 2., 0.), 0.05, 0.25,
Eigen::Quaterniond(0., 0., 0., 1.))); transform::Rigid3d(Eigen::Vector3d(1., 2., 0.),
mapping::proto::Submap proto; Eigen::Quaterniond(0., 0., 0., 1.)));
proto::Submap proto;
expected.ToProto(&proto, true /* include_probability_grid_data */); expected.ToProto(&proto, true /* include_probability_grid_data */);
EXPECT_FALSE(proto.has_submap_2d()); EXPECT_FALSE(proto.has_submap_2d());
EXPECT_TRUE(proto.has_submap_3d()); EXPECT_TRUE(proto.has_submap_3d());
const auto actual = Submap(proto.submap_3d()); const auto actual = Submap3D(proto.submap_3d());
EXPECT_TRUE(expected.local_pose().translation().isApprox( EXPECT_TRUE(expected.local_pose().translation().isApprox(
actual.local_pose().translation(), 1e-6)); actual.local_pose().translation(), 1e-6));
EXPECT_TRUE(expected.local_pose().rotation().isApprox( EXPECT_TRUE(expected.local_pose().rotation().isApprox(
@ -43,5 +44,5 @@ TEST(SubmapsTest, ToFromProto) {
} }
} // namespace } // namespace
} // namespace mapping_3d } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -103,7 +103,7 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
Eigen::Array3i point; Eigen::Array3i point;
int index; int index;
}; };
using Blocks = mapping_3d::HybridGridBase<std::vector<RasterPoint>>; using Blocks = mapping::HybridGridBase<std::vector<RasterPoint>>;
Blocks blocks(kPrecision); Blocks blocks(kPrecision);
int num_blocks = 0; int num_blocks = 0;
CHECK_LE(point_cloud.size(), std::numeric_limits<int>::max()); CHECK_LE(point_cloud.size(), std::numeric_limits<int>::max());

View File

@ -78,8 +78,7 @@ PointCloud AdaptivelyVoxelFiltered(
template <typename PointCloudType> template <typename PointCloudType>
PointCloudType FilterPointCloudUsingVoxels( PointCloudType FilterPointCloudUsingVoxels(
const PointCloudType& point_cloud, const PointCloudType& point_cloud, mapping::HybridGridBase<uint8>* voxels) {
mapping_3d::HybridGridBase<uint8>* voxels) {
PointCloudType results; PointCloudType results;
for (const typename PointCloudType::value_type& point : point_cloud) { for (const typename PointCloudType::value_type& point : point_cloud) {
auto* const value = auto* const value =

View File

@ -43,7 +43,7 @@ class VoxelFilter {
TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud); TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
private: private:
mapping_3d::HybridGridBase<uint8> voxels_; mapping::HybridGridBase<uint8> voxels_;
}; };
proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(

View File

@ -103,23 +103,23 @@ MapBuilderContext::UpdateSubmap2D(
return submap_2d_ptr; return submap_2d_ptr;
} }
std::shared_ptr<cartographer::mapping_3d::Submap> std::shared_ptr<cartographer::mapping::Submap3D>
MapBuilderContext::UpdateSubmap3D( MapBuilderContext::UpdateSubmap3D(
const cartographer::mapping::proto::Submap& proto) { const cartographer::mapping::proto::Submap& proto) {
CHECK(proto.has_submap_3d()); CHECK(proto.has_submap_3d());
cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(), cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
proto.submap_id().submap_index()}; proto.submap_id().submap_index()};
std::shared_ptr<cartographer::mapping_3d::Submap> submap_3d_ptr; std::shared_ptr<cartographer::mapping::Submap3D> submap_3d_ptr;
auto submap_it = unfinished_submaps_.find(submap_id); auto submap_it = unfinished_submaps_.find(submap_id);
if (submap_it == unfinished_submaps_.end()) { if (submap_it == unfinished_submaps_.end()) {
// Seeing a submap for the first time it should never be finished. // Seeing a submap for the first time it should never be finished.
CHECK(!proto.submap_3d().finished()); CHECK(!proto.submap_3d().finished());
submap_3d_ptr = submap_3d_ptr =
std::make_shared<cartographer::mapping_3d::Submap>(proto.submap_3d()); std::make_shared<cartographer::mapping::Submap3D>(proto.submap_3d());
unfinished_submaps_.Insert(submap_id, submap_3d_ptr); unfinished_submaps_.Insert(submap_id, submap_3d_ptr);
submap_it = unfinished_submaps_.find(submap_id); submap_it = unfinished_submaps_.find(submap_id);
} else { } else {
submap_3d_ptr = std::dynamic_pointer_cast<cartographer::mapping_3d::Submap>( submap_3d_ptr = std::dynamic_pointer_cast<cartographer::mapping::Submap3D>(
submap_it->data); submap_it->data);
CHECK(submap_3d_ptr); CHECK(submap_3d_ptr);
@ -157,8 +157,7 @@ MapBuilderContext::ProcessLocalSlamResultData(
cartographer::mapping::FromProto(proto.node_data())), cartographer::mapping::FromProto(proto.node_data())),
submaps); submaps);
} else { } else {
std::vector<std::shared_ptr<const cartographer::mapping_3d::Submap>> std::vector<std::shared_ptr<const cartographer::mapping::Submap3D>> submaps;
submaps;
for (const auto& submap_proto : proto.submaps()) { for (const auto& submap_proto : proto.submaps()) {
submaps.push_back(UpdateSubmap3D(submap_proto)); submaps.push_back(UpdateSubmap3D(submap_proto));
} }

View File

@ -18,7 +18,7 @@
#define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H #define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H
#include "cartographer/mapping_2d/submap_2d.h" #include "cartographer/mapping_2d/submap_2d.h"
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer_grpc/map_builder_context_interface.h" #include "cartographer_grpc/map_builder_context_interface.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -53,7 +53,7 @@ class MapBuilderContext : public MapBuilderContextInterface {
private: private:
std::shared_ptr<cartographer::mapping::Submap2D> UpdateSubmap2D( std::shared_ptr<cartographer::mapping::Submap2D> UpdateSubmap2D(
const cartographer::mapping::proto::Submap& proto); const cartographer::mapping::proto::Submap& proto);
std::shared_ptr<cartographer::mapping_3d::Submap> UpdateSubmap3D( std::shared_ptr<cartographer::mapping::Submap3D> UpdateSubmap3D(
const cartographer::mapping::proto::Submap& proto); const cartographer::mapping::proto::Submap& proto);
MapBuilderServer* map_builder_server_; MapBuilderServer* map_builder_server_;