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

Introduce TrajectoryBuilderInterface. (#736) 

[RFC=0004](https://github.com/googlecartographer/rfcs/blob/master/text/0004-trajectory-builder-interface.md)
master
Christoph Schütte 2017-12-08 11:52:29 +01:00 committed by Wally B. Feed
parent 96cdbde5bf
commit 3ae78563c6
17 changed files with 170 additions and 299 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
cartographer/internal/mapping/global_trajectory_builder.h
View File

@ -17,7 +17,7 @@
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_ #ifndef CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
#include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -26,8 +26,7 @@ namespace mapping {
template <typename LocalTrajectoryBuilder, template <typename LocalTrajectoryBuilder,
typename LocalTrajectoryBuilderOptions, typename PoseGraph> typename LocalTrajectoryBuilderOptions, typename PoseGraph>
class GlobalTrajectoryBuilder class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
: public mapping::GlobalTrajectoryBuilderInterface {
public: public:
GlobalTrajectoryBuilder( GlobalTrajectoryBuilder(
const LocalTrajectoryBuilderOptions& options, const int trajectory_id, const LocalTrajectoryBuilderOptions& options, const int trajectory_id,
@ -42,12 +41,15 @@ class GlobalTrajectoryBuilder
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
void AddRangefinderData(const common::Time time, void AddSensorData(
const Eigen::Vector3f& origin, const std::string& sensor_id,
const sensor::TimedPointCloud& ranges) override { const sensor::TimedPointCloudData& timed_point_cloud_data) override {
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_.AddRangeData( matching_result = local_trajectory_builder_.AddRangeData(
time, sensor::TimedRangeData{origin, ranges, {}}); timed_point_cloud_data.time,
sensor::TimedRangeData{timed_point_cloud_data.origin,
timed_point_cloud_data.ranges,
{}});
if (matching_result == nullptr) { if (matching_result == nullptr) {
// The range data has not been fully accumulated yet. // The range data has not been fully accumulated yet.
return; return;
@ -67,18 +69,21 @@ class GlobalTrajectoryBuilder
} }
} }
void AddSensorData(const sensor::ImuData& imu_data) override { void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
local_trajectory_builder_.AddImuData(imu_data); local_trajectory_builder_.AddImuData(imu_data);
pose_graph_->AddImuData(trajectory_id_, imu_data); pose_graph_->AddImuData(trajectory_id_, imu_data);
} }
void AddSensorData(const sensor::OdometryData& odometry_data) override { void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
CHECK(odometry_data.pose.IsValid()) << odometry_data.pose; CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
local_trajectory_builder_.AddOdometryData(odometry_data); local_trajectory_builder_.AddOdometryData(odometry_data);
pose_graph_->AddOdometryData(trajectory_id_, odometry_data); pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
} }
void AddSensorData( void AddSensorData(
const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose) override { const sensor::FixedFramePoseData& fixed_frame_pose) override {
CHECK(fixed_frame_pose.pose.IsValid()) << fixed_frame_pose.pose; CHECK(fixed_frame_pose.pose.IsValid()) << fixed_frame_pose.pose;
pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);

9
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -31,8 +31,7 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
sensor::Collator* const sensor_collator, const int trajectory_id, sensor::Collator* const sensor_collator, const int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids, const std::unordered_set<std::string>& expected_sensor_ids,
std::unique_ptr<GlobalTrajectoryBuilderInterface> std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
wrapped_trajectory_builder)
: sensor_collator_(sensor_collator), : sensor_collator_(sensor_collator),
trajectory_id_(trajectory_id), trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
@ -47,8 +46,8 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {} CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
void CollatedTrajectoryBuilder::AddSensorData( void CollatedTrajectoryBuilder::AddSensorData(
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) { std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data)); sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
} }
void CollatedTrajectoryBuilder::HandleCollatedSensorData( void CollatedTrajectoryBuilder::HandleCollatedSensorData(
@ -72,7 +71,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
last_logging_time_ = std::chrono::steady_clock::now(); last_logging_time_ = std::chrono::steady_clock::now();
} }
data->AddToGlobalTrajectoryBuilder(wrapped_trajectory_builder_.get()); data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
} }
} // namespace mapping } // namespace mapping

50
cartographer/mapping/collated_trajectory_builder.h
View File

@ -25,9 +25,8 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/rate_timer.h" #include "cartographer/common/rate_timer.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/sensor/collator.h" #include "cartographer/sensor/collator.h"
#include "cartographer/sensor/data.h" #include "cartographer/sensor/data.h"
@ -35,59 +34,50 @@ namespace cartographer {
namespace mapping { namespace mapping {
// Handles collating sensor data using a sensor::Collator, then passing it on to // Handles collating sensor data using a sensor::Collator, then passing it on to
// a mapping::GlobalTrajectoryBuilderInterface which is common for 2D and 3D. // a mapping::TrajectoryBuilderInterface which is common for 2D and 3D.
class CollatedTrajectoryBuilder : public TrajectoryBuilder { class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
public: public:
CollatedTrajectoryBuilder( CollatedTrajectoryBuilder(
sensor::Collator* sensor_collator, int trajectory_id, sensor::Collator* sensor_collator, int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids, const std::unordered_set<std::string>& expected_sensor_ids,
std::unique_ptr<GlobalTrajectoryBuilderInterface> std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder);
wrapped_trajectory_builder);
~CollatedTrajectoryBuilder() override; ~CollatedTrajectoryBuilder() override;
CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete; CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete;
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) = CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
delete; delete;
void AddRangefinderData(const std::string& sensor_id, common::Time time, void AddSensorData(
const Eigen::Vector3f& origin, const std::string& sensor_id,
const sensor::TimedPointCloud& ranges) override { const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddSensorData(sensor_id, AddSensorData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
common::make_unique<sensor::DispatchableRangefinderData>(
time, origin, ranges));
} }
void AddImuData(const std::string& sensor_id, common::Time time, void AddSensorData(const std::string& sensor_id,
const Eigen::Vector3d& linear_acceleration, const sensor::ImuData& imu_data) override {
const Eigen::Vector3d& angular_velocity) override { AddSensorData(sensor::MakeDispatchable(sensor_id, imu_data));
AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{
time, linear_acceleration, angular_velocity}));
} }
void AddOdometerData(const std::string& sensor_id, common::Time time, void AddSensorData(const std::string& sensor_id,
const transform::Rigid3d& odometer_pose) override { const sensor::OdometryData& odometry_data) override {
AddSensorData(sensor_id, sensor::MakeDispatchable( AddSensorData(sensor::MakeDispatchable(sensor_id, odometry_data));
sensor::OdometryData{time, odometer_pose}));
} }
void AddFixedFramePoseData( void AddSensorData(
const std::string& sensor_id, common::Time time, const std::string& sensor_id,
const transform::Rigid3d& fixed_frame_pose) override { const sensor::FixedFramePoseData& fixed_frame_pose_data) override {
AddSensorData(sensor_id, AddSensorData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
sensor::MakeDispatchable(
sensor::FixedFramePoseData{time, fixed_frame_pose}));
} }
private: private:
void AddSensorData(const std::string& sensor_id, void AddSensorData(std::unique_ptr<sensor::Data> data);
std::unique_ptr<sensor::Data> data);
void HandleCollatedSensorData(const std::string& sensor_id, void HandleCollatedSensorData(const std::string& sensor_id,
std::unique_ptr<sensor::Data> data); std::unique_ptr<sensor::Data> data);
sensor::Collator* const sensor_collator_; sensor::Collator* const sensor_collator_;
const int trajectory_id_; const int trajectory_id_;
std::unique_ptr<GlobalTrajectoryBuilderInterface> wrapped_trajectory_builder_; std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_;
// Time at which we last logged the rates of incoming sensor data. // Time at which we last logged the rates of incoming sensor data.
std::chrono::steady_clock::time_point last_logging_time_; std::chrono::steady_clock::time_point last_logging_time_;

2
cartographer/mapping/map_builder.cc
View File

@ -121,7 +121,7 @@ int MapBuilder::AddTrajectoryForDeserialization() {
return trajectory_id; return trajectory_id;
} }
TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder( TrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
const int trajectory_id) const { const int trajectory_id) const {
return trajectory_builders_.at(trajectory_id).get(); return trajectory_builders_.at(trajectory_id).get();
} }

5
cartographer/mapping/map_builder.h
View File

@ -52,7 +52,7 @@ class MapBuilder : public MapBuilderInterface {
int AddTrajectoryForDeserialization() override; int AddTrajectoryForDeserialization() override;
mapping::TrajectoryBuilder* GetTrajectoryBuilder( mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
int trajectory_id) const override; int trajectory_id) const override;
void FinishTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override;
@ -77,7 +77,8 @@ class MapBuilder : public MapBuilderInterface {
mapping::PoseGraph* pose_graph_; mapping::PoseGraph* pose_graph_;
sensor::Collator sensor_collator_; sensor::Collator sensor_collator_;
std::vector<std::unique_ptr<mapping::TrajectoryBuilder>> trajectory_builders_; std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
trajectory_builders_;
}; };
} // namespace mapping } // namespace mapping

9
cartographer/mapping/map_builder_interface.h
View File

@ -25,13 +25,12 @@
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder_interface.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
@ -41,7 +40,7 @@ namespace mapping {
class MapBuilderInterface { class MapBuilderInterface {
public: public:
using LocalSlamResultCallback = using LocalSlamResultCallback =
GlobalTrajectoryBuilderInterface::LocalSlamResultCallback; TrajectoryBuilderInterface::LocalSlamResultCallback;
MapBuilderInterface() {} MapBuilderInterface() {}
virtual ~MapBuilderInterface() {} virtual ~MapBuilderInterface() {}
@ -59,10 +58,10 @@ class MapBuilderInterface {
// builder for it will return 'nullptr'. // builder for it will return 'nullptr'.
virtual int AddTrajectoryForDeserialization() = 0; virtual int AddTrajectoryForDeserialization() = 0;
// Returns the TrajectoryBuilder corresponding to the specified // Returns the 'TrajectoryBuilderInterface' corresponding to the specified
// 'trajectory_id' or 'nullptr' if the trajectory has no corresponding // 'trajectory_id' or 'nullptr' if the trajectory has no corresponding
// builder. // builder.
virtual mapping::TrajectoryBuilder* GetTrajectoryBuilder( virtual mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
int trajectory_id) const = 0; int trajectory_id) const = 0;
// Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished, // Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished,

33
cartographer/mapping/map_builder_test.cc
View File

@ -24,7 +24,7 @@
#include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
namespace cartographer { namespace cartographer {
@ -37,13 +37,8 @@ constexpr double kDuration = 2.;
constexpr double kTimeStep = 0.1; constexpr double kTimeStep = 0.1;
constexpr double kTravelDistance = 0.4; constexpr double kTravelDistance = 0.4;
struct FakeRangeMeasurement { std::vector<sensor::TimedPointCloudData> GenerateFakeRangeMeasurements() {
common::Time time; std::vector<sensor::TimedPointCloudData> measurements;
sensor::TimedPointCloud ranges;
};
std::vector<FakeRangeMeasurement> GenerateFakeRangeMeasurements() {
std::vector<FakeRangeMeasurement> measurements;
sensor::TimedPointCloud point_cloud; sensor::TimedPointCloud point_cloud;
for (double angle = 0.; angle < M_PI; angle += 0.01) { for (double angle = 0.; angle < M_PI; angle += 0.01) {
constexpr double kRadius = 5; constexpr double kRadius = 5;
@ -60,7 +55,8 @@ std::vector<FakeRangeMeasurement> GenerateFakeRangeMeasurements() {
transform::Rigid3f::Translation(elapsed_time * kVelocity); transform::Rigid3f::Translation(elapsed_time * kVelocity);
sensor::TimedPointCloud ranges = sensor::TimedPointCloud ranges =
sensor::TransformTimedPointCloud(point_cloud, pose.inverse()); sensor::TransformTimedPointCloud(point_cloud, pose.inverse());
measurements.emplace_back(FakeRangeMeasurement{time, ranges}); measurements.emplace_back(
sensor::TimedPointCloudData{time, Eigen::Vector3f::Zero(), ranges});
} }
return measurements; return measurements;
} }
@ -160,13 +156,11 @@ TEST_F(MapBuilderTest, LocalSlam2D) {
int trajectory_id = map_builder_->AddTrajectoryBuilder( int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_builder_options_, expected_sensor_ids, trajectory_builder_options_,
GetLocalSlamResultCallback()); GetLocalSlamResultCallback());
TrajectoryBuilder* trajectory_builder = TrajectoryBuilderInterface* trajectory_builder =
map_builder_->GetTrajectoryBuilder(trajectory_id); map_builder_->GetTrajectoryBuilder(trajectory_id);
const auto measurements = GenerateFakeRangeMeasurements(); const auto measurements = GenerateFakeRangeMeasurements();
for (const auto& measurement : measurements) { for (const auto& measurement : measurements) {
trajectory_builder->AddRangefinderData(kRangeSensorId, measurement.time, trajectory_builder->AddSensorData(kRangeSensorId, measurement);
Eigen::Vector3f::Zero(),
measurement.ranges);
} }
map_builder_->FinishTrajectory(trajectory_id); map_builder_->FinishTrajectory(trajectory_id);
map_builder_->pose_graph()->RunFinalOptimization(); map_builder_->pose_graph()->RunFinalOptimization();
@ -186,16 +180,15 @@ TEST_F(MapBuilderTest, LocalSlam3D) {
int trajectory_id = map_builder_->AddTrajectoryBuilder( int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_builder_options_, expected_sensor_ids, trajectory_builder_options_,
GetLocalSlamResultCallback()); GetLocalSlamResultCallback());
TrajectoryBuilder* trajectory_builder = TrajectoryBuilderInterface* trajectory_builder =
map_builder_->GetTrajectoryBuilder(trajectory_id); map_builder_->GetTrajectoryBuilder(trajectory_id);
const auto measurements = GenerateFakeRangeMeasurements(); const auto measurements = GenerateFakeRangeMeasurements();
for (const auto& measurement : measurements) { for (const auto& measurement : measurements) {
trajectory_builder->AddRangefinderData(kRangeSensorId, measurement.time, trajectory_builder->AddSensorData(kRangeSensorId, measurement);
Eigen::Vector3f::Zero(), trajectory_builder->AddSensorData(
measurement.ranges); kIMUSensorId,
trajectory_builder->AddImuData(kIMUSensorId, measurement.time, sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8),
Eigen::Vector3d(0., 0., 9.8), Eigen::Vector3d::Zero()});
Eigen::Vector3d::Zero());
} }
map_builder_->FinishTrajectory(trajectory_id); map_builder_->FinishTrajectory(trajectory_id);
map_builder_->pose_graph()->RunFinalOptimization(); map_builder_->pose_graph()->RunFinalOptimization();

67
cartographer/mapping/trajectory_builder.h
View File

@ -1,67 +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.
*/
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_
#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_
#include <functional>
#include <memory>
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace mapping {
proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// This interface is used for both 2D and 3D SLAM.
class TrajectoryBuilder {
public:
TrajectoryBuilder() {}
virtual ~TrajectoryBuilder() {}
TrajectoryBuilder(const TrajectoryBuilder&) = delete;
TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
virtual void AddRangefinderData(const std::string& sensor_id,
common::Time time,
const Eigen::Vector3f& origin,
const sensor::TimedPointCloud& ranges) = 0;
virtual void AddImuData(const std::string& sensor_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;
virtual void AddOdometerData(const std::string& sensor_id, common::Time time,
const transform::Rigid3d& odometer_pose) = 0;
virtual void AddFixedFramePoseData(
const std::string& sensor_id, common::Time time,
const transform::Rigid3d& fixed_frame_pose) = 0;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_

2
cartographer/mapping/trajectory_builder.cc → cartographer/mapping/trajectory_builder_interface.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/mapping_2d/local_trajectory_builder_options.h" #include "cartographer/mapping_2d/local_trajectory_builder_options.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h" #include "cartographer/mapping_3d/local_trajectory_builder_options.h"

54
cartographer/mapping/global_trajectory_builder_interface.h → cartographer/mapping/trajectory_builder_interface.h
View File

@ -14,48 +14,36 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_ #ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_ #define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_
#include <functional> #include <functional>
#include <memory> #include <memory>
#include <string> #include <string>
#include <vector>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/timed_point_cloud_data.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// This interface is used for both 2D and 3D SLAM. Implementations wire up a // This interface is used for both 2D and 3D SLAM. Implementations wire up a
// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching // global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
// to detect loop closure, and a sparse pose graph optimization to compute // to detect loop closure, and a sparse pose graph optimization to compute
// optimized pose estimates. // optimized pose estimates.
class GlobalTrajectoryBuilderInterface { class TrajectoryBuilderInterface {
public: public:
GlobalTrajectoryBuilderInterface() {}
virtual ~GlobalTrajectoryBuilderInterface() {}
GlobalTrajectoryBuilderInterface(const GlobalTrajectoryBuilderInterface&) =
delete;
GlobalTrajectoryBuilderInterface& operator=(
const GlobalTrajectoryBuilderInterface&) = delete;
virtual void AddRangefinderData(common::Time time,
const Eigen::Vector3f& origin,
const sensor::TimedPointCloud& ranges) = 0;
virtual void AddSensorData(const sensor::ImuData& imu_data) = 0;
virtual void AddSensorData(const sensor::OdometryData& odometry_data) = 0;
virtual void AddSensorData(
const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
// A callback which is called after local SLAM processes an accumulated // A callback which is called after local SLAM processes an accumulated
// 'sensor::RangeData'. If the data was inserted into a submap, reports the // 'sensor::RangeData'. If the data was inserted into a submap, reports the
// assigned 'NodeId', otherwise 'nullptr' if the data was filtered out. // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
@ -64,9 +52,27 @@ class GlobalTrajectoryBuilderInterface {
transform::Rigid3d /* local pose estimate */, transform::Rigid3d /* local pose estimate */,
sensor::RangeData /* in local frame */, sensor::RangeData /* in local frame */,
std::unique_ptr<const mapping::NodeId>)>; std::unique_ptr<const mapping::NodeId>)>;
TrajectoryBuilderInterface() {}
virtual ~TrajectoryBuilderInterface() {}
TrajectoryBuilderInterface(const TrajectoryBuilderInterface&) = delete;
TrajectoryBuilderInterface& operator=(const TrajectoryBuilderInterface&) =
delete;
virtual void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) = 0;
virtual void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) = 0;
virtual void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) = 0;
virtual void AddSensorData(
const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
}; };
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_ #endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_

4
cartographer/sensor/collator.cc
View File

@ -40,9 +40,9 @@ void Collator::FinishTrajectory(const int trajectory_id) {
} }
void Collator::AddSensorData(const int trajectory_id, void Collator::AddSensorData(const int trajectory_id,
const std::string& sensor_id,
std::unique_ptr<Data> data) { std::unique_ptr<Data> data) {
queue_.Add(QueueKey{trajectory_id, sensor_id}, std::move(data)); QueueKey queue_key{trajectory_id, data->GetSensorId()};
queue_.Add(std::move(queue_key), std::move(data));
} }
void Collator::Flush() { queue_.Flush(); } void Collator::Flush() { queue_.Flush(); }

7
cartographer/sensor/collator.h
View File

@ -49,10 +49,9 @@ class Collator {
void FinishTrajectory(int trajectory_id); void FinishTrajectory(int trajectory_id);
// Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid // Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid
// sensor data. Sensor packets with matching 'sensor_id' must be added in time // sensor data. Sensor packets with matching 'data.sensor_id_' must be added
// order. // in time order.
void AddSensorData(int trajectory_id, const std::string& sensor_id, void AddSensorData(int trajectory_id, std::unique_ptr<Data> data);
std::unique_ptr<Data> data);
// Dispatches all queued sensor packets. May only be called once. // Dispatches all queued sensor packets. May only be called once.
// AddSensorData may not be called after Flush. // AddSensorData may not be called after Flush.

57
cartographer/sensor/collator_test.cc
View File

@ -23,6 +23,7 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/sensor/timed_point_cloud_data.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
namespace cartographer { namespace cartographer {
@ -32,17 +33,17 @@ namespace {
TEST(Collator, Ordering) { TEST(Collator, Ordering) {
const std::array<std::string, 4> kSensorId = { const std::array<std::string, 4> kSensorId = {
{"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}}; {"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
DispatchableRangefinderData zero(common::FromUniversal(0), TimedPointCloudData zero{
Eigen::Vector3f::Zero(), {}); common::FromUniversal(0), Eigen::Vector3f::Zero(), {}};
DispatchableRangefinderData first(common::FromUniversal(100), TimedPointCloudData first{
Eigen::Vector3f::Zero(), {}); common::FromUniversal(100), Eigen::Vector3f::Zero(), {}};
DispatchableRangefinderData second(common::FromUniversal(200), TimedPointCloudData second{
Eigen::Vector3f::Zero(), {}); common::FromUniversal(200), Eigen::Vector3f::Zero(), {}};
ImuData third{common::FromUniversal(300)}; ImuData third{common::FromUniversal(300)};
DispatchableRangefinderData fourth(common::FromUniversal(400), TimedPointCloudData fourth{
Eigen::Vector3f::Zero(), {}); common::FromUniversal(400), Eigen::Vector3f::Zero(), {}};
DispatchableRangefinderData fifth(common::FromUniversal(500), TimedPointCloudData fifth{
Eigen::Vector3f::Zero(), {}); common::FromUniversal(500), Eigen::Vector3f::Zero(), {}};
OdometryData sixth{common::FromUniversal(600), OdometryData sixth{common::FromUniversal(600),
transform::Rigid3d::Identity()}; transform::Rigid3d::Identity()};
@ -57,33 +58,17 @@ TEST(Collator, Ordering) {
constexpr int kTrajectoryId = 0; constexpr int kTrajectoryId = 0;
// Establish a common start time. // Establish a common start time.
collator.AddSensorData( collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], zero));
kTrajectoryId, kSensorId[0], collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], zero));
common::make_unique<DispatchableRangefinderData>(zero)); collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[2], zero));
collator.AddSensorData( collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[3], zero));
kTrajectoryId, kSensorId[1],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(
kTrajectoryId, kSensorId[2],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(
kTrajectoryId, kSensorId[3],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData( collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], first));
kTrajectoryId, kSensorId[0], collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[3], sixth));
common::make_unique<DispatchableRangefinderData>(first)); collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], fourth));
collator.AddSensorData(kTrajectoryId, kSensorId[3], MakeDispatchable(sixth)); collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], second));
collator.AddSensorData( collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], fifth));
kTrajectoryId, kSensorId[0], collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[2], third));
common::make_unique<DispatchableRangefinderData>(fourth));
collator.AddSensorData(
kTrajectoryId, kSensorId[1],
common::make_unique<DispatchableRangefinderData>(second));
collator.AddSensorData(
kTrajectoryId, kSensorId[1],
common::make_unique<DispatchableRangefinderData>(fifth));
collator.AddSensorData(kTrajectoryId, kSensorId[2], MakeDispatchable(third));
ASSERT_EQ(7, received.size()); ASSERT_EQ(7, received.size());
EXPECT_EQ(100, common::ToUniversal(received[4].second)); EXPECT_EQ(100, common::ToUniversal(received[4].second));

45
cartographer/sensor/data.cc
View File

@ -1,45 +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/sensor/data.h"
namespace cartographer {
namespace sensor {
template <>
void Dispatchable<sensor::ImuData>::AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) {
trajectory_builder->AddImuData(
sensor_id, data_.time, data_.linear_acceleration, data_.angular_velocity);
}
template <>
void Dispatchable<sensor::OdometryData>::AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) {
trajectory_builder->AddOdometerData(sensor_id, data_.time, data_.pose);
}
template <>
void Dispatchable<sensor::FixedFramePoseData>::AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) {
trajectory_builder->AddFixedFramePoseData(sensor_id, data_.time, data_.pose);
}
} // namespace sensor
} // namespace cartographer

59
cartographer/sensor/data.h
View File

@ -19,13 +19,7 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
@ -33,63 +27,38 @@ namespace sensor {
class Data { class Data {
public: public:
explicit Data(const std::string &sensor_id) : sensor_id_(sensor_id) {}
virtual ~Data() {} virtual ~Data() {}
virtual common::Time GetTime() const = 0; virtual common::Time GetTime() const = 0;
virtual void AddToGlobalTrajectoryBuilder( const std::string &GetSensorId() const { return sensor_id_; }
mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0;
virtual void AddToTrajectoryBuilder( virtual void AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder, mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;
const std::string& sensor_id) = 0;
};
class DispatchableRangefinderData : public Data { protected:
public: const std::string sensor_id_;
DispatchableRangefinderData(const common::Time time,
const Eigen::Vector3f& origin,
const TimedPointCloud& ranges)
: time_(time), origin_(origin), ranges_(ranges) {}
common::Time GetTime() const override { return time_; }
void AddToGlobalTrajectoryBuilder(
mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder)
override {
trajectory_builder->AddRangefinderData(time_, origin_, ranges_);
}
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) override {
trajectory_builder->AddRangefinderData(sensor_id, time_, origin_, ranges_);
}
private:
const common::Time time_;
const Eigen::Vector3f origin_;
const TimedPointCloud ranges_;
}; };
template <typename DataType> template <typename DataType>
class Dispatchable : public Data { class Dispatchable : public Data {
public: public:
Dispatchable(const DataType& data) : data_(data) {} Dispatchable(const std::string &sensor_id, const DataType &data)
: Data(sensor_id), data_(data) {}
common::Time GetTime() const override { return data_.time; } common::Time GetTime() const override { return data_.time; }
void AddToGlobalTrajectoryBuilder(
mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder)
override {
trajectory_builder->AddSensorData(data_);
}
void AddToTrajectoryBuilder( void AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder, mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
const std::string& sensor_id) override; trajectory_builder->AddSensorData(sensor_id_, data_);
}
private: private:
const DataType data_; const DataType data_;
}; };
template <typename DataType> template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(const DataType& data) { std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
return common::make_unique<Dispatchable<DataType>>(data); const std::string &sensor_id, const DataType &data) {
return common::make_unique<Dispatchable<DataType>>(sensor_id, data);
} }
} // namespace sensor } // namespace sensor

5
cartographer/sensor/ordered_multi_queue_test.cc
View File

@ -44,8 +44,9 @@ class OrderedMultiQueueTest : public ::testing::Test {
} }
std::unique_ptr<Data> MakeImu(const int ordinal) { std::unique_ptr<Data> MakeImu(const int ordinal) {
return MakeDispatchable(sensor::ImuData{common::FromUniversal(ordinal), return MakeDispatchable(
Eigen::Vector3d::Zero(), "imu",
sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero()}); Eigen::Vector3d::Zero()});
} }

36
cartographer/sensor/timed_point_cloud_data.h Normal file
View File

@ -0,0 +1,36 @@
/*
* Copyright 2017 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_SENSOR_TIMED_POINT_CLOUD_DATA_H_
#define CARTOGRAPHER_SENSOR_TIMED_POINT_CLOUD_DATA_H_
#include "Eigen/Core"
#include "cartographer/common/time.h"
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace sensor {
struct TimedPointCloudData {
common::Time time;
Eigen::Vector3f origin;
sensor::TimedPointCloud ranges;
};
} // namespace sensor
} // namespace cartographer
#endif // CARTOGRAPHER_SENSOR_TIMED_POINT_CLOUD_DATA_H_