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

Adds CollatedTrajectoryBuilder. (#82)

master
Wolfgang Hess 2016-10-19 16:29:26 +02:00 committed by GitHub
parent d4e04a3cda
commit e566bb73f5
8 changed files with 342 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
cartographer/mapping/CMakeLists.txt
View File

@ -15,15 +15,32 @@
add_subdirectory("proto")
add_subdirectory("sparse_pose_graph")
google_library(mapping_global_trajectory_builder_interface
google_library(mapping_collated_trajectory_builder
USES_GLOG
SRCS
global_trajectory_builder_interface.cc
collated_trajectory_builder.cc
HDRS
collated_trajectory_builder.h
DEPENDS
common_port
common_rate_timer
common_time
kalman_filter_pose_tracker
mapping_global_trajectory_builder_interface
mapping_submaps
mapping_trajectory_builder
sensor_collator
sensor_data
)
google_library(mapping_global_trajectory_builder_interface
HDRS
global_trajectory_builder_interface.h
DEPENDS
common_time
kalman_filter_pose_tracker
mapping_submaps
mapping_trajectory_builder
sensor_laser
sensor_point_cloud
)
@ -53,16 +70,14 @@ google_library(mapping_map_builder
common_make_unique
common_thread_pool
mapping_2d_global_trajectory_builder
mapping_2d_local_trajectory_builder
mapping_2d_sparse_pose_graph
mapping_2d_submaps
mapping_3d_global_trajectory_builder
mapping_3d_local_trajectory_builder_options
mapping_3d_proto_local_trajectory_builder_options
mapping_3d_sparse_pose_graph
mapping_global_trajectory_builder_interface
mapping_proto_map_builder_options
mapping_sparse_pose_graph
mapping_submaps
mapping_trajectory_node
sensor_laser
sensor_voxel_filter
@ -125,6 +140,20 @@ google_library(mapping_submaps
transform_transform
)
google_library(mapping_trajectory_builder
HDRS
trajectory_builder.h
DEPENDS
common_make_unique
common_port
common_time
kalman_filter_pose_tracker
mapping_submaps
sensor_data
sensor_laser
sensor_point_cloud
)
google_library(mapping_trajectory_connectivity
USES_GLOG
SRCS

112
cartographer/mapping/collated_trajectory_builder.cc Normal file
View File

@ -0,0 +1,112 @@
/*
* 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.
*/
#include "cartographer/mapping/collated_trajectory_builder.h"
#include "cartographer/common/time.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping {
namespace {
constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
} // namespace
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
sensor::Collator* const sensor_collator, const int trajectory_id,
const std::unordered_set<string>& expected_sensor_ids,
std::unique_ptr<GlobalTrajectoryBuilderInterface>
wrapped_trajectory_builder)
: sensor_collator_(sensor_collator),
trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
last_logging_time_(std::chrono::steady_clock::now()) {
sensor_collator_->AddTrajectory(
trajectory_id, expected_sensor_ids,
[this](const string& sensor_id, std::unique_ptr<sensor::Data> data) {
HandleCollatedSensorData(sensor_id, std::move(data));
});
}
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
const Submaps* CollatedTrajectoryBuilder::submaps() const {
return wrapped_trajectory_builder_->submaps();
}
Submaps* CollatedTrajectoryBuilder::submaps() {
return wrapped_trajectory_builder_->submaps();
}
kalman_filter::PoseTracker* CollatedTrajectoryBuilder::pose_tracker() const {
return wrapped_trajectory_builder_->pose_tracker();
}
const TrajectoryBuilder::PoseEstimate&
CollatedTrajectoryBuilder::pose_estimate() const {
return wrapped_trajectory_builder_->pose_estimate();
}
void CollatedTrajectoryBuilder::AddSensorData(
const string& sensor_id, std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data));
}
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
const string& sensor_id, std::unique_ptr<sensor::Data> data) {
auto it = rate_timers_.find(sensor_id);
if (it == rate_timers_.end()) {
it = rate_timers_
.emplace(
std::piecewise_construct, std::forward_as_tuple(sensor_id),
std::forward_as_tuple(
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
.first;
}
it->second.Pulse(data->time);
if (std::chrono::steady_clock::now() - last_logging_time_ >
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
for (const auto& pair : rate_timers_) {
LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
}
last_logging_time_ = std::chrono::steady_clock::now();
}
switch (data->type) {
case sensor::Data::Type::kImu:
wrapped_trajectory_builder_->AddImuData(data->time,
data->imu.linear_acceleration,
data->imu.angular_velocity);
return;
case sensor::Data::Type::kLaserFan:
wrapped_trajectory_builder_->AddLaserFan(data->time, data->laser_fan);
return;
case sensor::Data::Type::kOdometry:
wrapped_trajectory_builder_->AddOdometerPose(
data->time, data->odometry.pose, data->odometry.covariance);
return;
}
LOG(FATAL);
}
} // namespace mapping
} // namespace cartographer

77
cartographer/mapping/collated_trajectory_builder.h Normal file
View File

@ -0,0 +1,77 @@
/*
* 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_COLLATED_TRAJECTORY_BUILDER_H_
#define CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_
#include <chrono>
#include <map>
#include <memory>
#include <string>
#include <unordered_set>
#include "cartographer/common/port.h"
#include "cartographer/common/rate_timer.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/collator.h"
#include "cartographer/sensor/data.h"
namespace cartographer {
namespace mapping {
// Handles collating sensor data using a sensor::Collator, then passing it on to
// a mapping::GlobalTrajectoryBuilderInterface which is common for 2D and 3D.
class CollatedTrajectoryBuilder : public TrajectoryBuilder {
public:
CollatedTrajectoryBuilder(
sensor::Collator* sensor_collator, int trajectory_id,
const std::unordered_set<string>& expected_sensor_ids,
std::unique_ptr<GlobalTrajectoryBuilderInterface>
wrapped_trajectory_builder);
~CollatedTrajectoryBuilder() override;
CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete;
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
delete;
const Submaps* submaps() const override;
Submaps* submaps() override;
kalman_filter::PoseTracker* pose_tracker() const override;
const PoseEstimate& pose_estimate() const override;
void AddSensorData(const string& sensor_id,
std::unique_ptr<sensor::Data> data) override;
private:
void HandleCollatedSensorData(const string& sensor_id,
std::unique_ptr<sensor::Data> data);
sensor::Collator* const sensor_collator_;
const int trajectory_id_;
std::unique_ptr<GlobalTrajectoryBuilderInterface> wrapped_trajectory_builder_;
// Time at which we last logged the rates of incoming sensor data.
std::chrono::steady_clock::time_point last_logging_time_;
std::map<string, common::RateTimer<>> rate_timers_;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_

35
cartographer/mapping/global_trajectory_builder_interface.cc
View File

@ -1,35 +0,0 @@
/*
* 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.
*/
#include "cartographer/mapping/global_trajectory_builder_interface.h"
namespace cartographer {
namespace mapping {
GlobalTrajectoryBuilderInterface::PoseEstimate::PoseEstimate(
const common::Time time, const kalman_filter::PoseAndCovariance& prediction,
const kalman_filter::PoseAndCovariance& observation,
const kalman_filter::PoseAndCovariance& estimate,
const transform::Rigid3d& pose, const sensor::PointCloud& point_cloud)
: time(time),
prediction(prediction),
observation(observation),
estimate(estimate),
pose(pose),
point_cloud(point_cloud) {}
} // namespace mapping
} // namespace cartographer

30
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -24,6 +24,7 @@
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h"
@ -36,34 +37,7 @@ namespace mapping {
// optimized pose estimates.
class GlobalTrajectoryBuilderInterface {
public:
// Represents a newly computed pose. Each of the following steps in the pose
// estimation pipeline are provided for debugging:
//
// 1. UKF prediction
// 2. Absolute pose observation (e.g. from scan matching)
// 3. UKF estimate after integrating any measurements
//
// Finally, '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 kalman_filter::PoseAndCovariance& prediction,
const kalman_filter::PoseAndCovariance& observation,
const kalman_filter::PoseAndCovariance& estimate,
const transform::Rigid3d& pose,
const sensor::PointCloud& point_cloud);
common::Time time = common::Time::min();
kalman_filter::PoseAndCovariance prediction = {
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
kalman_filter::PoseAndCovariance observation = {
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
kalman_filter::PoseAndCovariance estimate = {
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
transform::Rigid3d pose = transform::Rigid3d::Identity();
sensor::PointCloud point_cloud;
};
using PoseEstimate = TrajectoryBuilder::PoseEstimate;
GlobalTrajectoryBuilderInterface() {}
virtual ~GlobalTrajectoryBuilderInterface() {}

4
cartographer/mapping/map_builder.h
View File

@ -28,11 +28,9 @@
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/proto/map_builder_options.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_2d/local_trajectory_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h"
namespace cartographer {

114
cartographer/mapping/trajectory_builder.h Normal file
View File

@ -0,0 +1,114 @@
/*
* 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_TRAJECTORY_BUILDER_H_
#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_
#include <functional>
#include <memory>
#include <string>
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/data.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace mapping {
// This interface is used for both 2D and 3D SLAM.
class TrajectoryBuilder {
public:
// Represents a newly computed pose. Each of the following steps in the pose
// estimation pipeline are provided for debugging:
//
// 1. UKF prediction
// 2. Absolute pose observation (e.g. from scan matching)
// 3. UKF estimate after integrating any measurements
//
// Finally, '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 kalman_filter::PoseAndCovariance& prediction,
const kalman_filter::PoseAndCovariance& observation,
const kalman_filter::PoseAndCovariance& estimate,
const transform::Rigid3d& pose,
const sensor::PointCloud& point_cloud)
: time(time),
prediction(prediction),
observation(observation),
estimate(estimate),
pose(pose),
point_cloud(point_cloud) {}
common::Time time = common::Time::min();
kalman_filter::PoseAndCovariance prediction = {
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
kalman_filter::PoseAndCovariance observation = {
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
kalman_filter::PoseAndCovariance estimate = {
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
transform::Rigid3d pose = transform::Rigid3d::Identity();
sensor::PointCloud point_cloud;
};
TrajectoryBuilder() {}
virtual ~TrajectoryBuilder() {}
TrajectoryBuilder(const TrajectoryBuilder&) = delete;
TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
virtual const Submaps* submaps() const = 0;
virtual Submaps* submaps() = 0;
virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0;
virtual void AddSensorData(const string& sensor_id,
std::unique_ptr<sensor::Data> data) = 0;
void AddLaserFan(const string& sensor_id, common::Time time,
const sensor::LaserFan& laser_fan) {
AddSensorData(sensor_id,
common::make_unique<sensor::Data>(time, laser_fan));
}
void AddImuData(const string& sensor_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
AddSensorData(sensor_id, common::make_unique<sensor::Data>(
time, sensor::Data::Imu{linear_acceleration,
angular_velocity}));
}
void AddOdometerPose(const string& sensor_id, common::Time time,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
AddSensorData(sensor_id,
common::make_unique<sensor::Data>(
time, sensor::Data::Odometry{pose, covariance}));
}
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_

3
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -40,7 +40,8 @@ class GlobalTrajectoryBuilder
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
const override;
// Handles approximately horizontal laser fans.
// Projects 'laser_fan' to 2D, and therefore should be approximately
// horizontal.
void AddLaserFan(common::Time time,
const sensor::LaserFan& laser_fan) override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,