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

Keep fixed frame across optimizations. (#807) 

PAIR=wohe
master
Susanne Pielawa 2018-01-10 16:01:27 +01:00 committed by Wally B. Feed
parent 286d16238e
commit c7a8c5fda9
2 changed files with 31 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -378,7 +378,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
} }
// Add fixed frame pose constraints. // Add fixed frame pose constraints.
std::deque<CeresPose> C_fixed_frames; std::map<int, CeresPose> C_fixed_frames;
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id; const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
@ -387,6 +387,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
continue; continue;
} }
const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
bool fixed_frame_pose_initialized = false; bool fixed_frame_pose_initialized = false;
for (; node_it != trajectory_end; ++node_it) { for (; node_it != trajectory_end; ++node_it) {
const mapping::NodeId node_id = node_it->id; const mapping::NodeId node_id = node_it->id;
@ -403,26 +404,34 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
options_.fixed_frame_pose_rotation_weight()}; options_.fixed_frame_pose_rotation_weight()};
if (!fixed_frame_pose_initialized) { if (!fixed_frame_pose_initialized) {
const transform::Rigid3d fixed_frame_pose_in_map = transform::Rigid3d fixed_frame_pose_in_map;
node_data.global_pose * constraint_pose.zbar_ij.inverse(); if (trajectory_data.fixed_frame.has_value()) {
C_fixed_frames.emplace_back( fixed_frame_pose_in_map = trajectory_data.fixed_frame.value();
transform::Rigid3d( } else {
fixed_frame_pose_in_map.translation(), fixed_frame_pose_in_map =
Eigen::AngleAxisd( node_data.global_pose * constraint_pose.zbar_ij.inverse();
transform::GetYaw(fixed_frame_pose_in_map.rotation()), }
Eigen::Vector3d::UnitZ())), C_fixed_frames.emplace(
nullptr, std::piecewise_construct, std::forward_as_tuple(trajectory_id),
common::make_unique<ceres::AutoDiffLocalParameterization< std::forward_as_tuple(
YawOnlyQuaternionPlus, 4, 1>>(), transform::Rigid3d(
&problem); fixed_frame_pose_in_map.translation(),
Eigen::AngleAxisd(
transform::GetYaw(fixed_frame_pose_in_map.rotation()),
Eigen::Vector3d::UnitZ())),
nullptr,
common::make_unique<ceres::AutoDiffLocalParameterization<
YawOnlyQuaternionPlus, 4, 1>>(),
&problem));
fixed_frame_pose_initialized = true; fixed_frame_pose_initialized = true;
} }
problem.AddResidualBlock( problem.AddResidualBlock(
SpaCostFunction::CreateAutoDiffCostFunction(constraint_pose), SpaCostFunction::CreateAutoDiffCostFunction(constraint_pose),
nullptr /* loss function */, C_fixed_frames.back().rotation(), nullptr /* loss function */,
C_fixed_frames.back().translation(), C_nodes.at(node_id).rotation(), C_fixed_frames.at(trajectory_id).rotation(),
C_nodes.at(node_id).translation()); C_fixed_frames.at(trajectory_id).translation(),
C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation());
} }
} }
@ -458,6 +467,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
node_data_.at(C_node_id_data.id).global_pose = node_data_.at(C_node_id_data.id).global_pose =
C_node_id_data.data.ToRigid(); C_node_id_data.data.ToRigid();
} }
for (const auto& C_fixed_frame : C_fixed_frames) {
trajectory_data_.at(C_fixed_frame.first).fixed_frame =
C_fixed_frame.second.ToRigid();
}
} }
const mapping::MapById<mapping::NodeId, NodeData>& const mapping::MapById<mapping::NodeId, NodeData>&

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

@ -24,6 +24,7 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/optional.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
@ -105,6 +106,7 @@ class OptimizationProblem {
struct TrajectoryData { struct TrajectoryData {
double gravity_constant = 9.8; double gravity_constant = 9.8;
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}}; std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
common::optional<transform::Rigid3d> fixed_frame;
}; };
mapping::pose_graph::proto::OptimizationProblemOptions options_; mapping::pose_graph::proto::OptimizationProblemOptions options_;