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) {
case sensor::Data::Type::kImu:
wrapped_trajectory_builder_->AddImuData(data->time,
data->imu.linear_acceleration,
data->imu.angular_velocity);
wrapped_trajectory_builder_->AddImuData(
sensor::ImuData{data->time, data->imu.linear_acceleration,
data->imu.angular_velocity});
return;
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/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"
#include "cartographer/transform/rigid_transform.h"
@ -53,9 +54,7 @@ class GlobalTrajectoryBuilderInterface {
virtual void AddRangefinderData(common::Time time,
const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) = 0;
virtual void AddImuData(common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;
virtual void AddImuData(const sensor::ImuData& imu_data) = 0;
virtual void AddOdometerData(common::Time time,
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));
}
void GlobalTrajectoryBuilder::AddImuData(
const common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
local_trajectory_builder_.AddImuData(time, linear_acceleration,
angular_velocity);
sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
angular_velocity);
void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
local_trajectory_builder_.AddImuData(imu_data);
sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
}
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.
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
void AddImuData(const sensor::ImuData& imu_data) override;
void AddOdometerData(common::Time time,
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_;
}
void LocalTrajectoryBuilder::AddImuData(
const common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
InitializeImuTracker(time);
Predict(time);
imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
imu_tracker_->AddImuAngularVelocityObservation(angular_velocity);
InitializeImuTracker(imu_data.time);
Predict(imu_data.time);
imu_tracker_->AddImuLinearAccelerationObservation(
imu_data.linear_acceleration);
imu_tracker_->AddImuAngularVelocityObservation(imu_data.angular_velocity);
}
void LocalTrajectoryBuilder::AddOdometerData(

3
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -58,8 +58,7 @@ class LocalTrajectoryBuilder {
const PoseEstimate& pose_estimate() const;
std::unique_ptr<InsertionResult> AddHorizontalRangeData(
common::Time, const sensor::RangeData& range_data);
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddImuData(const sensor::ImuData& imu_data);
void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
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,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
void SparsePoseGraph::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration,
angular_velocity);
optimization_problem_.AddImuData(trajectory_id, imu_data);
});
}

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -77,9 +77,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void FreezeTrajectory(int trajectory_id) override;
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() {}
void OptimizationProblem::AddImuData(const int trajectory_id,
const common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
const sensor::ImuData& imu_data) {
CHECK_GE(trajectory_id, 0);
imu_data_.resize(
std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back(
sensor::ImuData{time, linear_acceleration, angular_velocity});
imu_data_[trajectory_id].push_back(imu_data);
}
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& operator=(const OptimizationProblem&) = delete;
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid2d& initial_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() {}
void GlobalTrajectoryBuilder::AddImuData(
const common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
local_trajectory_builder_.AddImuData(time, linear_acceleration,
angular_velocity);
sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
angular_velocity);
void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
local_trajectory_builder_.AddImuData(imu_data);
sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void GlobalTrajectoryBuilder::AddRangefinderData(

3
cartographer/mapping_3d/global_trajectory_builder.h
View File

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

13
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -42,17 +42,16 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
void LocalTrajectoryBuilder::AddImuData(
const common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
const bool initial_imu_data = (imu_tracker_ == nullptr);
if (initial_imu_data) {
imu_tracker_ = common::make_unique<mapping::ImuTracker>(
options_.imu_gravity_time_constant(), time);
options_.imu_gravity_time_constant(), imu_data.time);
}
Predict(time);
imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
imu_tracker_->AddImuAngularVelocityObservation(angular_velocity);
Predict(imu_data.time);
imu_tracker_->AddImuLinearAccelerationObservation(
imu_data.linear_acceleration);
imu_tracker_->AddImuAngularVelocityObservation(imu_data.angular_velocity);
if (initial_imu_data) {
// This uses the first accelerometer measurement to approximately align the
// 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& operator=(const LocalTrajectoryBuilder&) = delete;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddImuData(const sensor::ImuData& imu_data);
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
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 Eigen::Vector3d gravity =
expected_pose.rotation().inverse() * Eigen::Vector3d(0., 0., 9.81);
local_trajectory_builder_->AddImuData(time, gravity,
Eigen::Vector3d::Zero());
local_trajectory_builder_->AddImuData(
sensor::ImuData{time, gravity, Eigen::Vector3d::Zero()});
}
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,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
void SparsePoseGraph::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration,
angular_velocity);
optimization_problem_.AddImuData(trajectory_id, imu_data);
});
}

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -75,9 +75,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void FreezeTrajectory(int trajectory_id) override;
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() {}
void OptimizationProblem::AddImuData(const int trajectory_id,
const common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
const sensor::ImuData& imu_data) {
CHECK_GE(trajectory_id, 0);
imu_data_.resize(
std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back(
sensor::ImuData{time, linear_acceleration, angular_velocity});
imu_data_[trajectory_id].push_back(imu_data);
}
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& operator=(const OptimizationProblem&) = delete;
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid3d& point_cloud_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) {
const transform::Rigid3d pose =
AddNoise(node.ground_truth_pose, node.noise);
optimization_problem_.AddImuData(kTrajectoryId, now,
Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero());
optimization_problem_.AddImuData(
kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero()});
optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose);
now += common::FromSeconds(0.01);
}