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

Pass IMU data as sensor::ImuData. (#441)

master
Wolfgang Hess 2017-08-08 14:27:08 +02:00 committed by GitHub
parent eb53b70fec
commit f60b2cbb16
20 changed files with 46 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -79,9 +79,9 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
switch (data->type) { switch (data->type) {
case sensor::Data::Type::kImu: case sensor::Data::Type::kImu:
wrapped_trajectory_builder_->AddImuData(data->time, wrapped_trajectory_builder_->AddImuData(
data->imu.linear_acceleration, sensor::ImuData{data->time, data->imu.linear_acceleration,
data->imu.angular_velocity); data->imu.angular_velocity});
return; return;
case sensor::Data::Type::kRangefinder: case sensor::Data::Type::kRangefinder:

5
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -25,6 +25,7 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -53,9 +54,7 @@ class GlobalTrajectoryBuilderInterface {
virtual void AddRangefinderData(common::Time time, virtual void AddRangefinderData(common::Time time,
const Eigen::Vector3f& origin, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) = 0; const sensor::PointCloud& ranges) = 0;
virtual void AddImuData(common::Time time, virtual void AddImuData(const sensor::ImuData& imu_data) = 0;
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;
virtual void AddOdometerData(common::Time time, virtual void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) = 0; const transform::Rigid3d& pose) = 0;
}; };

10
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -44,13 +44,9 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
std::move(insertion_result->insertion_submaps)); std::move(insertion_result->insertion_submaps));
} }
void GlobalTrajectoryBuilder::AddImuData( void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
const common::Time time, const Eigen::Vector3d& linear_acceleration, local_trajectory_builder_.AddImuData(imu_data);
const Eigen::Vector3d& angular_velocity) { sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
local_trajectory_builder_.AddImuData(time, linear_acceleration,
angular_velocity);
sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
angular_velocity);
} }
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

3
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -42,8 +42,7 @@ class GlobalTrajectoryBuilder
// parallel to the ground plane. // parallel to the ground plane.
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override; const sensor::PointCloud& ranges) override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(const sensor::ImuData& imu_data) override;
const Eigen::Vector3d& angular_velocity) override;
void AddOdometerData(common::Time time, void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override; const transform::Rigid3d& pose) override;

13
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -221,15 +221,14 @@ LocalTrajectoryBuilder::pose_estimate() const {
return last_pose_estimate_; return last_pose_estimate_;
} }
void LocalTrajectoryBuilder::AddImuData( void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
const common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added."; CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
InitializeImuTracker(time); InitializeImuTracker(imu_data.time);
Predict(time); Predict(imu_data.time);
imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration); imu_tracker_->AddImuLinearAccelerationObservation(
imu_tracker_->AddImuAngularVelocityObservation(angular_velocity); imu_data.linear_acceleration);
imu_tracker_->AddImuAngularVelocityObservation(imu_data.angular_velocity);
} }
void LocalTrajectoryBuilder::AddOdometerData( void LocalTrajectoryBuilder::AddOdometerData(

3
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -58,8 +58,7 @@ class LocalTrajectoryBuilder {
const PoseEstimate& pose_estimate() const; const PoseEstimate& pose_estimate() const;
std::unique_ptr<InsertionResult> AddHorizontalRangeData( std::unique_ptr<InsertionResult> AddHorizontalRangeData(
common::Time, const sensor::RangeData& range_data); common::Time, const sensor::RangeData& range_data);
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(const sensor::ImuData& imu_data);
const Eigen::Vector3d& angular_velocity);
void AddOdometerData(common::Time time, const transform::Rigid3d& pose); void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
private: private:

8
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -156,13 +156,11 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
} }
} }
void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time, void SparsePoseGraph::AddImuData(const int trajectory_id,
const Eigen::Vector3d& linear_acceleration, const sensor::ImuData& imu_data) {
const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration, optimization_problem_.AddImuData(trajectory_id, imu_data);
angular_velocity);
}); });
} }

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -77,9 +77,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_); EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization. // Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time, void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(int trajectory_id, void AddSubmapFromProto(int trajectory_id,

7
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -61,14 +61,11 @@ OptimizationProblem::OptimizationProblem(
OptimizationProblem::~OptimizationProblem() {} OptimizationProblem::~OptimizationProblem() {}
void OptimizationProblem::AddImuData(const int trajectory_id, void OptimizationProblem::AddImuData(const int trajectory_id,
const common::Time time, const sensor::ImuData& imu_data) {
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
imu_data_.resize( imu_data_.resize(
std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1)); std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back( imu_data_[trajectory_id].push_back(imu_data);
sensor::ImuData{time, linear_acceleration, angular_velocity});
} }
void OptimizationProblem::AddTrajectoryNode( void OptimizationProblem::AddTrajectoryNode(

4
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -58,9 +58,7 @@ class OptimizationProblem {
OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete;
void AddImuData(int trajectory_id, common::Time time, void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(int trajectory_id, common::Time time, void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose); const transform::Rigid2d& point_cloud_pose);

10
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -28,13 +28,9 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
void GlobalTrajectoryBuilder::AddImuData( void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
const common::Time time, const Eigen::Vector3d& linear_acceleration, local_trajectory_builder_.AddImuData(imu_data);
const Eigen::Vector3d& angular_velocity) { sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
local_trajectory_builder_.AddImuData(time, linear_acceleration,
angular_velocity);
sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
angular_velocity);
} }
void GlobalTrajectoryBuilder::AddRangefinderData( void GlobalTrajectoryBuilder::AddRangefinderData(

3
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -36,8 +36,7 @@ class GlobalTrajectoryBuilder
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(const sensor::ImuData& imu_data) override;
const Eigen::Vector3d& angular_velocity) override;
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override; const sensor::PointCloud& ranges) override;
void AddOdometerData(common::Time time, void AddOdometerData(common::Time time,

13
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -42,17 +42,16 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
void LocalTrajectoryBuilder::AddImuData( void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
const common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
const bool initial_imu_data = (imu_tracker_ == nullptr); const bool initial_imu_data = (imu_tracker_ == nullptr);
if (initial_imu_data) { if (initial_imu_data) {
imu_tracker_ = common::make_unique<mapping::ImuTracker>( imu_tracker_ = common::make_unique<mapping::ImuTracker>(
options_.imu_gravity_time_constant(), time); options_.imu_gravity_time_constant(), imu_data.time);
} }
Predict(time); Predict(imu_data.time);
imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration); imu_tracker_->AddImuLinearAccelerationObservation(
imu_tracker_->AddImuAngularVelocityObservation(angular_velocity); imu_data.linear_acceleration);
imu_tracker_->AddImuAngularVelocityObservation(imu_data.angular_velocity);
if (initial_imu_data) { if (initial_imu_data) {
// This uses the first accelerometer measurement to approximately align the // This uses the first accelerometer measurement to approximately align the
// first pose to gravity. // first pose to gravity.

3
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -55,8 +55,7 @@ class LocalTrajectoryBuilder {
LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(const sensor::ImuData& imu_data);
const Eigen::Vector3d& angular_velocity);
std::unique_ptr<InsertionResult> AddRangefinderData( std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin, common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges); const sensor::PointCloud& ranges);

4
cartographer/mapping_3d/local_trajectory_builder_test.cc
View File

@ -208,8 +208,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
const transform::Rigid3d& expected_pose) { const transform::Rigid3d& expected_pose) {
const Eigen::Vector3d gravity = const Eigen::Vector3d gravity =
expected_pose.rotation().inverse() * Eigen::Vector3d(0., 0., 9.81); expected_pose.rotation().inverse() * Eigen::Vector3d(0., 0., 9.81);
local_trajectory_builder_->AddImuData(time, gravity, local_trajectory_builder_->AddImuData(
Eigen::Vector3d::Zero()); sensor::ImuData{time, gravity, Eigen::Vector3d::Zero()});
} }
std::vector<TrajectoryNode> GenerateCorkscrewTrajectory() { std::vector<TrajectoryNode> GenerateCorkscrewTrajectory() {

8
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -147,13 +147,11 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
} }
} }
void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time, void SparsePoseGraph::AddImuData(const int trajectory_id,
const Eigen::Vector3d& linear_acceleration, const sensor::ImuData& imu_data) {
const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration, optimization_problem_.AddImuData(trajectory_id, imu_data);
angular_velocity);
}); });
} }

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -75,9 +75,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_); EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization. // Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time, void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(int trajectory_id, void AddSubmapFromProto(int trajectory_id,

7
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -80,14 +80,11 @@ OptimizationProblem::OptimizationProblem(
OptimizationProblem::~OptimizationProblem() {} OptimizationProblem::~OptimizationProblem() {}
void OptimizationProblem::AddImuData(const int trajectory_id, void OptimizationProblem::AddImuData(const int trajectory_id,
const common::Time time, const sensor::ImuData& imu_data) {
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
imu_data_.resize( imu_data_.resize(
std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1)); std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back( imu_data_[trajectory_id].push_back(imu_data);
sensor::ImuData{time, linear_acceleration, angular_velocity});
} }
void OptimizationProblem::AddTrajectoryNode( void OptimizationProblem::AddTrajectoryNode(

4
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

@ -60,9 +60,7 @@ class OptimizationProblem {
OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete;
void AddImuData(int trajectory_id, common::Time time, void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(int trajectory_id, common::Time time, void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid3d& point_cloud_pose); const transform::Rigid3d& point_cloud_pose);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);

6
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -122,9 +122,9 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
for (const NoisyNode& node : test_data) { for (const NoisyNode& node : test_data) {
const transform::Rigid3d pose = const transform::Rigid3d pose =
AddNoise(node.ground_truth_pose, node.noise); AddNoise(node.ground_truth_pose, node.noise);
optimization_problem_.AddImuData(kTrajectoryId, now, optimization_problem_.AddImuData(
Eigen::Vector3d::UnitZ() * 9.81, kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero()); Eigen::Vector3d::Zero()});
optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose); optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose);
now += common::FromSeconds(0.01); now += common::FromSeconds(0.01);
} }