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

Pass odometry data to the 3D optimization problem. (#495) 

Not yet used.
master
Wolfgang Hess 2017-09-01 09:58:24 +02:00 committed by GitHub
parent 72bb24e362
commit d3c49c8585
3 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -152,8 +152,10 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
void SparsePoseGraph::AddOdometerData( void SparsePoseGraph::AddOdometerData(
const int trajectory_id, const sensor::OdometryData& odometry_data) { const int trajectory_id, const sensor::OdometryData& odometry_data) {
// TODO(cschuet): Add support for handling OdometryData in the optimization common::MutexLocker locker(&mutex_);
// problem. AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddOdometerData(trajectory_id, odometry_data);
});
} }
void SparsePoseGraph::AddFixedFramePoseData( void SparsePoseGraph::AddFixedFramePoseData(

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

@ -61,6 +61,14 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
imu_data_[trajectory_id].push_back(imu_data); 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<size_t>(trajectory_id) + 1));
odometry_data_[trajectory_id].Push(odometry_data.time, odometry_data.pose);
}
void OptimizationProblem::AddFixedFramePoseData( void OptimizationProblem::AddFixedFramePoseData(
const int trajectory_id, const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) { const sensor::FixedFramePoseData& fixed_frame_pose_data) {

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

@ -31,6 +31,7 @@
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.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/transform/transform_interpolation_buffer.h" #include "cartographer/transform/transform_interpolation_buffer.h"
namespace cartographer { namespace cartographer {
@ -64,6 +65,8 @@ class OptimizationProblem {
OptimizationProblem& operator=(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete;
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data);
void AddFixedFramePoseData( void AddFixedFramePoseData(
int trajectory_id, int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data); const sensor::FixedFramePoseData& fixed_frame_pose_data);
@ -91,6 +94,7 @@ class OptimizationProblem {
FixZ fix_z_; FixZ fix_z_;
std::vector<std::deque<sensor::ImuData>> imu_data_; std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::vector<NodeData>> node_data_; std::vector<std::vector<NodeData>> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_;
std::vector<std::vector<SubmapData>> submap_data_; std::vector<std::vector<SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_; std::vector<TrajectoryData> trajectory_data_;
std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_; std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;