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

Extract PoseEstimate into its own file. (#474) 

This is to remove the dependency of GlobalTrajectoryBuilderInterface
on the TrajectoryBuilder.
master
Wolfgang Hess 2017-08-24 12:04:39 +02:00 committed by GitHub
parent f0e1dab031
commit e78e2cb5ad
12 changed files with 61 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -46,8 +46,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
const TrajectoryBuilder::PoseEstimate&
CollatedTrajectoryBuilder::pose_estimate() const {
const PoseEstimate& CollatedTrajectoryBuilder::pose_estimate() const {
return wrapped_trajectory_builder_->pose_estimate();
}

4
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -23,8 +23,8 @@
#include <vector>
#include "cartographer/common/time.h"
#include "cartographer/mapping/pose_estimate.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
@ -39,8 +39,6 @@ namespace mapping {
// optimized pose estimates.
class GlobalTrajectoryBuilderInterface {
public:
using PoseEstimate = TrajectoryBuilder::PoseEstimate;
GlobalTrajectoryBuilderInterface() {}
virtual ~GlobalTrajectoryBuilderInterface() {}

43
cartographer/mapping/pose_estimate.h Normal file
View File

@ -0,0 +1,43 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_POSE_ESTIMATE_H_
#define CARTOGRAPHER_MAPPING_POSE_ESTIMATE_H_
#include "cartographer/common/time.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace mapping {
// Represents a newly computed pose. 'pose' is the end-user visualization of
// orientation and 'point_cloud' is the point cloud, in the local map frame.
struct PoseEstimate {
PoseEstimate() = default;
PoseEstimate(common::Time time, const transform::Rigid3d& pose,
const sensor::PointCloud& point_cloud)
: time(time), pose(pose), point_cloud(point_cloud) {}
common::Time time = common::Time::min();
transform::Rigid3d pose = transform::Rigid3d::Identity();
sensor::PointCloud point_cloud;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_POSE_ESTIMATE_H_

14
cartographer/mapping/trajectory_builder.h
View File

@ -25,6 +25,7 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/pose_estimate.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/data.h"
@ -40,18 +41,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
// This interface is used for both 2D and 3D SLAM.
class TrajectoryBuilder {
public:
// Represents a newly computed pose. 'pose' is the end-user visualization of
// orientation and 'point_cloud' is the point cloud, in the local map frame.
struct PoseEstimate {
PoseEstimate() = default;
PoseEstimate(common::Time time, const transform::Rigid3d& pose,
const sensor::PointCloud& point_cloud)
: time(time), pose(pose), point_cloud(point_cloud) {}
common::Time time = common::Time::min();
transform::Rigid3d pose = transform::Rigid3d::Identity();
sensor::PointCloud point_cloud;
};
using PoseEstimate = mapping::PoseEstimate;
TrajectoryBuilder() {}
virtual ~TrajectoryBuilder() {}

3
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -56,8 +56,7 @@ void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
sensor::OdometryData{time, pose});
}
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&
GlobalTrajectoryBuilder::pose_estimate() const {
const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
return local_trajectory_builder_.pose_estimate();
}

3
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -35,8 +35,7 @@ class GlobalTrajectoryBuilder
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
const override;
const mapping::PoseEstimate& pose_estimate() const override;
// Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately
// parallel to the ground plane.

3
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -186,8 +186,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
range_data_in_tracking_2d, pose_estimate_2d});
}
const LocalTrajectoryBuilder::PoseEstimate&
LocalTrajectoryBuilder::pose_estimate() const {
const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
return last_pose_estimate_;
}

10
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -33,12 +33,10 @@
namespace cartographer {
namespace mapping_2d {
// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
// closure.
// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
// without loop closure.
class LocalTrajectoryBuilder {
public:
using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
struct InsertionResult {
common::Time time;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
@ -54,7 +52,7 @@ class LocalTrajectoryBuilder {
LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
const PoseEstimate& pose_estimate() const;
const mapping::PoseEstimate& pose_estimate() const;
std::unique_ptr<InsertionResult> AddHorizontalRangeData(
common::Time, const sensor::RangeData& range_data);
void AddImuData(const sensor::ImuData& imu_data);
@ -81,7 +79,7 @@ class LocalTrajectoryBuilder {
const proto::LocalTrajectoryBuilderOptions options_;
ActiveSubmaps active_submaps_;
PoseEstimate last_pose_estimate_;
mapping::PoseEstimate last_pose_estimate_;
mapping_3d::MotionFilter motion_filter_;
scan_matching::RealTimeCorrelativeScanMatcher

3
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -52,8 +52,7 @@ void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
local_trajectory_builder_.AddOdometerData(time, pose);
}
const GlobalTrajectoryBuilder::PoseEstimate&
GlobalTrajectoryBuilder::pose_estimate() const {
const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
return local_trajectory_builder_.pose_estimate();
}

2
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -41,7 +41,7 @@ class GlobalTrajectoryBuilder
const sensor::PointCloud& ranges) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
const PoseEstimate& pose_estimate() const override;
const mapping::PoseEstimate& pose_estimate() const override;
private:
const int trajectory_id_;

3
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -175,8 +175,7 @@ void LocalTrajectoryBuilder::AddOdometerData(
extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose});
}
const LocalTrajectoryBuilder::PoseEstimate&
LocalTrajectoryBuilder::pose_estimate() const {
const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
return last_pose_estimate_;
}

10
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -34,12 +34,10 @@
namespace cartographer {
namespace mapping_3d {
// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
// closure.
// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
// without loop closure.
class LocalTrajectoryBuilder {
public:
using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
struct InsertionResult {
common::Time time;
sensor::RangeData range_data_in_tracking;
@ -60,7 +58,7 @@ class LocalTrajectoryBuilder {
const sensor::PointCloud& ranges);
void AddOdometerData(common::Time time,
const transform::Rigid3d& odometer_pose);
const PoseEstimate& pose_estimate() const;
const mapping::PoseEstimate& pose_estimate() const;
private:
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
@ -73,7 +71,7 @@ class LocalTrajectoryBuilder {
const proto::LocalTrajectoryBuilderOptions options_;
ActiveSubmaps active_submaps_;
PoseEstimate last_pose_estimate_;
mapping::PoseEstimate last_pose_estimate_;
MotionFilter motion_filter_;
std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>