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

In optimization_problem, use a map instead of vector for trajectory data. (#805) 

PAIR=wohe
master
Susanne Pielawa 2018-01-10 14:30:51 +01:00 committed by Wally B. Feed
parent 67d26747cc
commit 286d16238e
2 changed files with 11 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
cartographer/mapping_3d/pose_graph/optimization_problem.cc
View File

@ -126,20 +126,16 @@ void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const transform::Rigid3d& local_pose, const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose) { const transform::Rigid3d& global_pose) {
CHECK_GE(trajectory_id, 0);
trajectory_data_.resize(std::max(trajectory_data_.size(),
static_cast<size_t>(trajectory_id) + 1));
node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose}); node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose});
trajectory_data_[trajectory_id];
} }
void OptimizationProblem::InsertTrajectoryNode( void OptimizationProblem::InsertTrajectoryNode(
const mapping::NodeId& node_id, const common::Time time, const mapping::NodeId& node_id, const common::Time time,
const transform::Rigid3d& local_pose, const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose) { const transform::Rigid3d& global_pose) {
CHECK_GE(node_id.trajectory_id, 0);
trajectory_data_.resize(std::max(
trajectory_data_.size(), static_cast<size_t>(node_id.trajectory_id) + 1));
node_data_.Insert(node_id, NodeData{time, local_pose, global_pose}); node_data_.Insert(node_id, NodeData{time, local_pose, global_pose});
trajectory_data_[node_id.trajectory_id];
} }
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
@ -147,23 +143,19 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
odometry_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id);
fixed_frame_pose_data_.Trim(node_data_, node_id); fixed_frame_pose_data_.Trim(node_data_, node_id);
node_data_.Trim(node_id); node_data_.Trim(node_id);
if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) {
trajectory_data_.erase(node_id.trajectory_id);
}
} }
void OptimizationProblem::AddSubmap( void OptimizationProblem::AddSubmap(
const int trajectory_id, const transform::Rigid3d& global_submap_pose) { const int trajectory_id, const transform::Rigid3d& global_submap_pose) {
CHECK_GE(trajectory_id, 0);
trajectory_data_.resize(std::max(trajectory_data_.size(),
static_cast<size_t>(trajectory_id) + 1));
submap_data_.Append(trajectory_id, SubmapData{global_submap_pose}); submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
} }
void OptimizationProblem::InsertSubmap( void OptimizationProblem::InsertSubmap(
const mapping::SubmapId& submap_id, const mapping::SubmapId& submap_id,
const transform::Rigid3d& global_submap_pose) { const transform::Rigid3d& global_submap_pose) {
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_data_.resize(
std::max(trajectory_data_.size(),
static_cast<size_t>(submap_id.trajectory_id) + 1));
submap_data_.Insert(submap_id, SubmapData{global_submap_pose}); submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
} }
@ -441,15 +433,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
&problem, &summary); &problem, &summary);
if (options_.log_solver_summary()) { if (options_.log_solver_summary()) {
LOG(INFO) << summary.FullReport(); LOG(INFO) << summary.FullReport();
for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size(); for (const auto& trajectory_id_and_data : trajectory_data_) {
++trajectory_id) { const int trajectory_id = trajectory_id_and_data.first;
const TrajectoryData& trajectory_data = trajectory_id_and_data.second;
if (trajectory_id != 0) { if (trajectory_id != 0) {
LOG(INFO) << "Trajectory " << trajectory_id << ":"; LOG(INFO) << "Trajectory " << trajectory_id << ":";
} }
LOG(INFO) << "Gravity was: " LOG(INFO) << "Gravity was: " << trajectory_data.gravity_constant;
<< trajectory_data_[trajectory_id].gravity_constant; const auto& imu_calibration = trajectory_data.imu_calibration;
const auto& imu_calibration =
trajectory_data_[trajectory_id].imu_calibration;
LOG(INFO) << "IMU correction was: " LOG(INFO) << "IMU correction was: "
<< common::RadToDeg(2. * std::acos(imu_calibration[0])) << common::RadToDeg(2. * std::acos(imu_calibration[0]))
<< " deg (" << imu_calibration[0] << ", " << imu_calibration[1] << " deg (" << imu_calibration[0] << ", " << imu_calibration[1]

3
cartographer/mapping_3d/pose_graph/optimization_problem.h
View File

@ -18,7 +18,6 @@
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#include <array> #include <array>
#include <deque>
#include <map> #include <map>
#include <set> #include <set>
#include <vector> #include <vector>
@ -115,7 +114,7 @@ class OptimizationProblem {
sensor::MapByTime<sensor::ImuData> imu_data_; sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_; sensor::MapByTime<sensor::OdometryData> odometry_data_;
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_; sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
std::vector<TrajectoryData> trajectory_data_; std::map<int, TrajectoryData> trajectory_data_;
}; };
} // namespace pose_graph } // namespace pose_graph