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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

10
cartographer/internal/mapping_3d/local_trajectory_builder_test.cc
View File

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

4
cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h
View File

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

9
cartographer/io/hybrid_grid_points_processor.cc
View File

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

10
cartographer/io/hybrid_grid_points_processor.h
View File

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

2
cartographer/io/outlier_removing_points_processor.h
View File

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

4
cartographer/io/submap_painter.cc
View File

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

4
cartographer/io/xray_points_processor.cc
View File

@ -111,7 +111,7 @@ XRayPointsProcessor::XRayPointsProcessor(
transform_(transform) {
for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
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 ysize = bounding_box_.sizes()[2] + 1;
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()) {
const Eigen::Array3i cell_index = it.GetCellIndex();
const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index);

2
cartographer/io/xray_points_processor.h
View File

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

2
cartographer/mapping/local_slam_result_data.cc
View File

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

5
cartographer/mapping/local_slam_result_data.h
View File

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

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

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

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

@ -16,7 +16,7 @@ syntax = "proto3";
import "cartographer/transform/proto/transform.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;
@ -28,8 +28,7 @@ message InitialTrajectoryPose {
message TrajectoryBuilderOptions {
LocalTrajectoryBuilderOptions2D trajectory_builder_2d_options = 1;
mapping_3d.proto.LocalTrajectoryBuilderOptions
trajectory_builder_3d_options = 2;
LocalTrajectoryBuilderOptions3D trajectory_builder_3d_options = 2;
bool pure_localization = 3;
InitialTrajectoryPose initial_trajectory_pose = 4;
}

4
cartographer/mapping/trajectory_builder_interface.cc
View File

@ -18,7 +18,7 @@
#include "cartographer/mapping/local_slam_result_data.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 mapping {
@ -30,7 +30,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
CreateLocalTrajectoryBuilderOptions2D(
parameter_dictionary->GetDictionary("trajectory_builder_2d").get());
*options.mutable_trajectory_builder_3d_options() =
mapping_3d::CreateLocalTrajectoryBuilderOptions(
CreateLocalTrajectoryBuilderOptions3D(
parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
options.set_pure_localization(
parameter_dictionary->GetBool("pure_localization"));

20
cartographer/mapping_3d/hybrid_grid.h
View File

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

40
cartographer/mapping_3d/hybrid_grid_test.cc
View File

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

20
cartographer/mapping_3d/local_trajectory_builder_options.cc → cartographer/mapping_3d/local_trajectory_builder_options_3d.cc
View File

@ -14,21 +14,21 @@
* 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/mapping_2d/scan_matching/real_time_correlative_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 "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions options;
proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
common::LuaParameterDictionary* parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions3D options;
options.set_min_range(parameter_dictionary->GetDouble("min_range"));
options.set_max_range(parameter_dictionary->GetDouble("max_range"));
options.set_num_accumulated_range_data(
@ -53,18 +53,18 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
->GetDictionary("real_time_correlative_scan_matcher")
.get());
*options.mutable_ceres_scan_matcher_options() =
scan_matching::CreateCeresScanMatcherOptions(
mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
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());
options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
options.set_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());
return options;
}
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

14
cartographer/mapping_3d/local_trajectory_builder_options.h → cartographer/mapping_3d/local_trajectory_builder_options_3d.h
View File

@ -14,19 +14,19 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#define 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_3D_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 mapping_3d {
namespace mapping {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
common::LuaParameterDictionary* parameter_dictionary);
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_

9
cartographer/mapping_3d/pose_graph/constraint_builder.cc
View File

@ -55,7 +55,7 @@ ConstraintBuilder::~ConstraintBuilder() {
}
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::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
@ -82,7 +82,7 @@ void ConstraintBuilder::MaybeAddConstraint(
}
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::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
@ -123,7 +123,8 @@ void ConstraintBuilder::WhenDone(
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id,
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 !=
nullptr) {
thread_pool_->Schedule(work_item);
@ -140,7 +141,7 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
void ConstraintBuilder::ConstructSubmapScanMatcher(
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Submap* const submap) {
const mapping::Submap3D* const submap) {
auto submap_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
submap->high_resolution_hybrid_grid(),

14
cartographer/mapping_3d/pose_graph/constraint_builder.h
View File

@ -36,7 +36,7 @@
#include "cartographer/mapping/trajectory_node.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/submaps.h"
#include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/point_cloud.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
// all computations are finished.
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::TrajectoryNode::Data* const constant_data,
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
// all computations are finished.
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::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
@ -114,8 +114,8 @@ class ConstraintBuilder {
private:
struct SubmapScanMatcher {
const HybridGrid* high_resolution_hybrid_grid;
const HybridGrid* low_resolution_hybrid_grid;
const mapping::HybridGrid* high_resolution_hybrid_grid;
const mapping::HybridGrid* low_resolution_hybrid_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
fast_correlative_scan_matcher;
};
@ -125,14 +125,14 @@ class ConstraintBuilder {
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id,
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_);
// Constructs the scan matcher for a 'submap', then schedules its work items.
void ConstructSubmapScanMatcher(
const mapping::SubmapId& submap_id,
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.
const SubmapScanMatcher* GetSubmapScanMatcher(

4
cartographer/mapping_3d/pose_graph/optimization_problem.cc
View File

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

16
cartographer/mapping_3d/pose_graph_3d.cc
View File

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

25
cartographer/mapping_3d/pose_graph_3d.h
View File

@ -37,7 +37,7 @@
#include "cartographer/mapping/trajectory_connectivity_state.h"
#include "cartographer/mapping_3d/pose_graph/constraint_builder.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/landmark_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
// 'insertion_submaps.front().finished()' is 'true', data was inserted into
// this submap for the last time.
NodeId AddNode(std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
const std::vector<std::shared_ptr<const mapping_3d::Submap>>&
insertion_submaps) EXCLUDES(mutex_);
NodeId AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
EXCLUDES(mutex_);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_);
@ -94,10 +95,10 @@ class PoseGraph3D : public PoseGraph {
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override;
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 AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) override;
void AddNodeToSubmap(const NodeId& node_id,
const SubmapId& submap_id) override;
void AddSerializedConstraints(
const std::vector<Constraint>& constraints) 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.
enum class SubmapState { kActive, kFinished };
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
// 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'.
std::vector<SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const mapping_3d::Submap>>&
insertion_submaps) REQUIRES(mutex_);
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
REQUIRES(mutex_);
// Adds constraints for a node, and starts scan matching in the background.
void ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps,
std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
bool newly_finished_submap) REQUIRES(mutex_);
// Computes constraints for a node and submap pair.

2
cartographer/mapping_3d/proto/hybrid_grid.proto
View File

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

10
cartographer/mapping_3d/proto/local_trajectory_builder_options.proto → cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto
View File

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

4
cartographer/mapping_3d/proto/range_data_inserter_options.proto → cartographer/mapping_3d/proto/range_data_inserter_options_3d.proto
View File

@ -14,9 +14,9 @@
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
// must be greater than 0.5).
double hit_probability = 1;

8
cartographer/mapping_3d/proto/submaps_options.proto → cartographer/mapping_3d/proto/submaps_options_3d.proto
View File

@ -14,11 +14,11 @@
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
// loop closure.
double high_resolution = 1;
@ -36,5 +36,5 @@ message SubmapsOptions {
// matched against, then while being matched.
int32 num_range_data = 2;
RangeDataInserterOptions range_data_inserter_options = 3;
RangeDataInserterOptions3D range_data_inserter_options = 3;
}

27
cartographer/mapping_3d/range_data_inserter.cc → cartographer/mapping_3d/range_data_inserter_3d.cc
View File

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

25
cartographer/mapping_3d/range_data_inserter.h → cartographer/mapping_3d/range_data_inserter_3d.h
View File

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

30
cartographer/mapping_3d/range_data_inserter_test.cc → cartographer/mapping_3d/range_data_inserter_3d_test.cc
View File

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

4
cartographer/mapping_3d/rotation_parameterization.h
View File

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

5
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc
View File

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

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

@ -35,7 +35,7 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
common::LuaParameterDictionary* parameter_dictionary);
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.
class CeresScanMatcher {

2
cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc
View File

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

6
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

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

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

@ -57,8 +57,8 @@ class FastCorrelativeScanMatcher {
};
FastCorrelativeScanMatcher(
const HybridGrid& hybrid_grid,
const HybridGrid* low_resolution_hybrid_grid,
const mapping::HybridGrid& hybrid_grid,
const mapping::HybridGrid* low_resolution_hybrid_grid,
const std::vector<mapping::TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions& options);
~FastCorrelativeScanMatcher();
@ -132,7 +132,7 @@ class FastCorrelativeScanMatcher {
const float resolution_;
const int width_in_voxels_;
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_;
};

13
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

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

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

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

2
cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc
View File

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

3
cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc
View File

@ -21,7 +21,8 @@ namespace mapping_3d {
namespace scan_matching {
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) {
float score = 0.f;
for (const Eigen::Vector3f& point :

3
cartographer/mapping_3d/scan_matching/low_resolution_matcher.h
View File

@ -28,7 +28,8 @@ namespace mapping_3d {
namespace scan_matching {
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 mapping_3d

6
cartographer/mapping_3d/scan_matching/precomputation_grid.cc
View File

@ -47,9 +47,11 @@ Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) {
} // namespace
PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) {
PrecomputationGrid ConvertToPrecomputationGrid(
const mapping::HybridGrid& hybrid_grid) {
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(
(mapping::ValueToProbability(it.GetValue()) -
mapping::kMinProbability) *

7
cartographer/mapping_3d/scan_matching/precomputation_grid.h
View File

@ -23,10 +23,10 @@ namespace cartographer {
namespace mapping_3d {
namespace scan_matching {
class PrecomputationGrid : public HybridGridBase<uint8> {
class PrecomputationGrid : public mapping::HybridGridBase<uint8> {
public:
explicit PrecomputationGrid(const float resolution)
: HybridGridBase<uint8>(resolution) {}
: mapping::HybridGridBase<uint8>(resolution) {}
// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
static float ToProbability(float value) {
@ -38,7 +38,8 @@ class PrecomputationGrid : public HybridGridBase<uint8> {
// Converts a HybridGrid to a PrecomputationGrid representing the same data,
// 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
// original voxels in 'grid'. This maximum is over the 8 voxels that have

2
cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc
View File

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

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

@ -34,7 +34,8 @@ RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
float RealTimeCorrelativeScanMatcher::Match(
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 {
CHECK_NOTNULL(pose_estimate);
float best_score = -1.f;
@ -96,7 +97,7 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
}
float RealTimeCorrelativeScanMatcher::ScoreCandidate(
const HybridGrid& hybrid_grid,
const mapping::HybridGrid& hybrid_grid,
const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const {
float score = 0.f;

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

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

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

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

68
cartographer/mapping_3d/submaps.cc → cartographer/mapping_3d/submap_3d.cc
View File

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

51
cartographer/mapping_3d/submaps.h → cartographer/mapping_3d/submap_3d.h
View File

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

17
cartographer/mapping_3d/submaps_test.cc → cartographer/mapping_3d/submap_3d_test.cc
View File

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

2
cartographer/sensor/compressed_point_cloud.cc
View File

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

3
cartographer/sensor/voxel_filter.cc
View File

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

2
cartographer/sensor/voxel_filter.h
View File

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

11
cartographer_grpc/map_builder_context.cc
View File

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

4
cartographer_grpc/map_builder_context.h
View File

@ -18,7 +18,7 @@
#define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_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"
namespace cartographer_grpc {
@ -53,7 +53,7 @@ class MapBuilderContext : public MapBuilderContextInterface {
private:
std::shared_ptr<cartographer::mapping::Submap2D> UpdateSubmap2D(
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);
MapBuilderServer* map_builder_server_;