diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index 74e386a..11c5cbf 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -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: diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 9f6e895..0d44971 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -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; }; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index a13b85d..a61b5a4 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -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, diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index 422adbf..eee14cc 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -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; diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index c421d1a..8f67302 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -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( diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index cb2b385..ce11ef3 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -58,8 +58,7 @@ class LocalTrajectoryBuilder { const PoseEstimate& pose_estimate() const; std::unique_ptr 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: diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index b0c70d6..567f03a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -156,13 +156,11 @@ void SparsePoseGraph::AddWorkItem(std::function 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); }); } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 6995993..fedeed1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -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, diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index a159139..65648fc 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -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(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( diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 209308b..41bfd6e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -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); diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 4dd55e0..b987bb5 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -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( diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index 5e229fc..98c43eb 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -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, diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index e4b1bce..098f760 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -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( - 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. diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 960e5cd..53dc325 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -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 AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges); diff --git a/cartographer/mapping_3d/local_trajectory_builder_test.cc b/cartographer/mapping_3d/local_trajectory_builder_test.cc index ec455ff..263199b 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_test.cc @@ -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 GenerateCorkscrewTrajectory() { diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index f0da401..beaff04 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -147,13 +147,11 @@ void SparsePoseGraph::AddWorkItem(std::function 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); }); } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index ddad6a8..d27b688 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -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, diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index e987b4a..28fe16d 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -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(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( diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 403c1c4..6d05c89 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -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); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 56426be..28b756e 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -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); }