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

Add fixed frame pose data to the 3D optimization problem. (#476)

master
jie 2017-08-24 15:02:23 +02:00 committed by Wolfgang Hess
parent a239b71a6e
commit 14977f73f1
3 changed files with 21 additions and 1 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

@ -158,7 +158,11 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
void SparsePoseGraph::AddFixedFramePoseData( void SparsePoseGraph::AddFixedFramePoseData(
const int trajectory_id, const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) { const sensor::FixedFramePoseData& fixed_frame_pose_data) {
LOG(FATAL) << "Not yet implemented for 3D."; common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data);
});
} }
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,

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

@ -87,6 +87,16 @@ 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::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
CHECK_GE(trajectory_id, 0);
fixed_frame_pose_data_.resize(std::max(
fixed_frame_pose_data_.size(), static_cast<size_t>(trajectory_id) + 1));
fixed_frame_pose_data_[trajectory_id].Push(fixed_frame_pose_data.time,
fixed_frame_pose_data.pose);
}
void OptimizationProblem::AddTrajectoryNode( void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const transform::Rigid3d& point_cloud_pose) { const transform::Rigid3d& point_cloud_pose) {

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

@ -29,7 +29,9 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#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/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
@ -61,6 +63,9 @@ 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 AddFixedFramePoseData(
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data);
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);
@ -86,6 +91,7 @@ class OptimizationProblem {
std::vector<std::vector<NodeData>> node_data_; std::vector<std::vector<NodeData>> node_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_;
}; };
} // namespace sparse_pose_graph } // namespace sparse_pose_graph