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

Refactoring: Do not pass 'constant_data' in pieces. (#488) 

Also removes the no longer used range data from trajectory nodes
in 2D.
master
Wolfgang Hess 2017-08-30 14:22:12 +02:00 committed by GitHub
parent 094b5a4d93
commit 3c22c8253a
11 changed files with 54 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -38,11 +38,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
return;
}
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d,
insertion_result->range_data_in_tracking_2d,
insertion_result->filtered_point_cloud_in_tracking_2d,
insertion_result->pose_estimate_2d, trajectory_id_,
insertion_result->insertion_submaps);
insertion_result->constant_data, insertion_result->pose_estimate_2d,
trajectory_id_, insertion_result->insertion_submaps);
}
void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {

15
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -17,6 +17,7 @@
#include "cartographer/mapping_2d/local_trajectory_builder.h"
#include <limits>
#include <memory>
#include "cartographer/common/make_unique.h"
#include "cartographer/sensor/range_data.h"
@ -186,10 +187,16 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
const sensor::PointCloud filtered_point_cloud_in_tracking_2d =
adaptive_voxel_filter.Filter(range_data_in_tracking_2d.returns);
return common::make_unique<InsertionResult>(
InsertionResult{time, std::move(insertion_submaps),
tracking_to_tracking_2d, range_data_in_tracking_2d,
pose_estimate_2d, filtered_point_cloud_in_tracking_2d});
return common::make_unique<InsertionResult>(InsertionResult{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time,
{}, // 'range_data' is only used in 3D.
filtered_point_cloud_in_tracking_2d,
{}, // 'high_resolution_point_cloud' is only used in 3D.
{}, // 'low_resolution_point_cloud' is only used in 3D.
tracking_to_tracking_2d}),
pose_estimate_2d, std::move(insertion_submaps)});
}
const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {

7
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -41,12 +41,9 @@ namespace mapping_2d {
class LocalTrajectoryBuilder {
public:
struct InsertionResult {
common::Time time;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
transform::Rigid3d tracking_to_tracking_2d;
sensor::RangeData range_data_in_tracking_2d;
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
transform::Rigid2d pose_estimate_2d;
sensor::PointCloud filtered_point_cloud_in_tracking_2d;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
};
explicit LocalTrajectoryBuilder(

15
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -94,9 +94,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
}
void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose,
const sensor::PointCloud& filtered_point_cloud,
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid2d& pose, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
@ -104,16 +102,7 @@ void SparsePoseGraph::AddScan(
common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append(
trajectory_id,
mapping::TrajectoryNode{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{time,
Compress(range_data_in_pose),
filtered_point_cloud,
{},
{},
tracking_to_pose}),
optimized_pose});
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);

13
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -64,16 +64,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' that
// will later be optimized. The 'tracking_to_pose' is remembered so that the
// optimized pose can be embedded into 3D. The 'pose' was determined by scan
// matching against the 'insertion_submaps.front()' and the scan was inserted
// into the 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
// Adds a new node with 'constant_data' and a 'pose' that will later be
// optimized. The 'pose' was determined by scan matching against
// 'insertion_submaps.front()' and the scan was inserted into the
// 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
// 'true', this submap was inserted into for the last time.
void AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose,
const sensor::PointCloud& filtered_point_cloud,
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid2d& pose, int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_);

10
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -158,8 +158,14 @@ class SparsePoseGraphTest : public ::testing::Test {
range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan(
common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
range_data.returns, pose_estimate, kTrajectoryId, insertion_submaps);
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{common::FromUniversal(0),
Compress(range_data),
range_data.returns,
{},
{},
transform::Rigid3d::Identity()}),
pose_estimate, kTrajectoryId, insertion_submaps);
}
void MoveRelative(const transform::Rigid2d& movement) {

9
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -31,17 +31,14 @@ GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
void GlobalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
auto insertion_result =
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddRangefinderData(time, origin, ranges);
if (insertion_result == nullptr) {
return;
}
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->range_data_in_tracking,
insertion_result->high_resolution_point_cloud,
insertion_result->low_resolution_point_cloud,
insertion_result->pose_observation, trajectory_id_,
insertion_result->insertion_submaps);
insertion_result->constant_data, insertion_result->pose_observation,
trajectory_id_, insertion_result->insertion_submaps);
}
void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {

15
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -16,6 +16,8 @@
#include "cartographer/mapping_3d/local_trajectory_builder.h"
#include <memory>
#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"
@ -201,9 +203,16 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
pose_observation.cast<float>()),
extrapolator_->gravity_orientation());
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, high_resolution_point_cloud,
low_resolution_point_cloud, pose_observation,
std::move(insertion_submaps)});
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time,
sensor::Compress(range_data_in_tracking),
{}, // 'filtered_point_cloud' is only used in 2D.
high_resolution_point_cloud,
low_resolution_point_cloud,
transform::Rigid3d::Identity()}),
pose_observation, std::move(insertion_submaps)});
}
} // namespace mapping_3d

8
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -41,13 +41,7 @@ namespace mapping_3d {
class LocalTrajectoryBuilder {
public:
struct InsertionResult {
common::Time time;
sensor::RangeData range_data_in_tracking;
// Used for loop closure in 3D.
sensor::PointCloud high_resolution_point_cloud;
sensor::PointCloud low_resolution_point_cloud;
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
transform::Rigid3d pose_observation;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
};

15
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -93,25 +93,14 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
}
void SparsePoseGraph::AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const sensor::PointCloud& high_resolution_point_cloud,
const sensor::PointCloud& low_resolution_point_cloud,
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid3d& pose, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time,
sensor::Compress(range_data_in_tracking),
{},
high_resolution_point_cloud,
low_resolution_point_cloud,
transform::Rigid3d::Identity()}),
optimized_pose});
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);

14
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -64,15 +64,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
// that will later be optimized. The 'pose' was determined by scan matching
// against 'insertion_submaps.front()' and the scan was inserted into the
// 'insertion_submaps'. If 'insertion_submaps.front().finished()' is 'true',
// this submap was inserted into for the last time.
// Adds a new node with 'constant_data' and a 'pose' that will later be
// optimized. The 'pose' was determined by scan matching against
// 'insertion_submaps.front()' and the scan was inserted into the
// 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
// 'true', this submap was inserted into for the last time.
void AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const sensor::PointCloud& high_resolution_point_cloud,
const sensor::PointCloud& low_resolution_point_cloud,
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid3d& pose, int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_);