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 transform::Rigid3d& local_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});
trajectory_data_[trajectory_id];
}
void OptimizationProblem::InsertTrajectoryNode(
const mapping::NodeId& node_id, const common::Time time,
const transform::Rigid3d& local_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});
trajectory_data_[node_id.trajectory_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);
fixed_frame_pose_data_.Trim(node_data_, 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(
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});
}
void OptimizationProblem::InsertSubmap(
const mapping::SubmapId& submap_id,
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});
}
@ -441,15 +433,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
&problem, &summary);
if (options_.log_solver_summary()) {
LOG(INFO) << summary.FullReport();
for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size();
++trajectory_id) {
for (const auto& trajectory_id_and_data : trajectory_data_) {
const int trajectory_id = trajectory_id_and_data.first;
const TrajectoryData& trajectory_data = trajectory_id_and_data.second;
if (trajectory_id != 0) {
LOG(INFO) << "Trajectory " << trajectory_id << ":";
}
LOG(INFO) << "Gravity was: "
<< trajectory_data_[trajectory_id].gravity_constant;
const auto& imu_calibration =
trajectory_data_[trajectory_id].imu_calibration;
LOG(INFO) << "Gravity was: " << trajectory_data.gravity_constant;
const auto& imu_calibration = trajectory_data.imu_calibration;
LOG(INFO) << "IMU correction was: "
<< common::RadToDeg(2. * std::acos(imu_calibration[0]))
<< " 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_
#include <array>
#include <deque>
#include <map>
#include <set>
#include <vector>
@ -115,7 +114,7 @@ class OptimizationProblem {
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
std::vector<TrajectoryData> trajectory_data_;
std::map<int, TrajectoryData> trajectory_data_;
};
} // namespace pose_graph