diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 80c835d..2c59b0f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -152,8 +152,10 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, void SparsePoseGraph::AddOdometerData( const int trajectory_id, const sensor::OdometryData& odometry_data) { - // TODO(cschuet): Add support for handling OdometryData in the optimization - // problem. + common::MutexLocker locker(&mutex_); + AddWorkItem([=]() REQUIRES(mutex_) { + optimization_problem_.AddOdometerData(trajectory_id, odometry_data); + }); } void SparsePoseGraph::AddFixedFramePoseData( diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index c4314cc..732247e 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -61,6 +61,14 @@ void OptimizationProblem::AddImuData(const int trajectory_id, imu_data_[trajectory_id].push_back(imu_data); } +void OptimizationProblem::AddOdometerData( + const int trajectory_id, const sensor::OdometryData& odometry_data) { + CHECK_GE(trajectory_id, 0); + odometry_data_.resize( + std::max(odometry_data_.size(), static_cast(trajectory_id) + 1)); + odometry_data_[trajectory_id].Push(odometry_data.time, odometry_data.pose); +} + void OptimizationProblem::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index dfa48fd..3745f93 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -31,6 +31,7 @@ #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/transform_interpolation_buffer.h" namespace cartographer { @@ -64,6 +65,8 @@ class OptimizationProblem { OptimizationProblem& operator=(const OptimizationProblem&) = delete; void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); + void AddOdometerData(int trajectory_id, + const sensor::OdometryData& odometry_data); void AddFixedFramePoseData( int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data); @@ -91,6 +94,7 @@ class OptimizationProblem { FixZ fix_z_; std::vector> imu_data_; std::vector> node_data_; + std::vector odometry_data_; std::vector> submap_data_; std::vector trajectory_data_; std::vector fixed_frame_pose_data_;