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

Add landmarks to the state and deserialize them. (#988)

master
Alexander Belyaev 2018-03-14 21:36:15 +01:00 committed by GitHub
parent 36df3eec19
commit 1e4d558ac4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 53 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc
View File

@ -74,10 +74,15 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes,
const MapById<NodeId, NodeData>& node_data,
bool freeze_landmarks, const MapById<NodeId, NodeData>& node_data,
MapById<NodeId, std::array<double, 3>>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
for (const auto& landmark_node : landmark_nodes) {
// Do not use landmarks that were not optimized for localization.
if (!landmark_node.second.global_landmark_pose.has_value() &&
freeze_landmarks) {
continue;
}
for (const auto& observation : landmark_node.second.landmark_observations) {
const std::string& landmark_id = landmark_node.first;
const auto& begin_of_trajectory =
@ -109,6 +114,12 @@ void AddLandmarkCostFunctions(
CeresPose(starting_point, nullptr /* translation_parametrization */,
common::make_unique<ceres::QuaternionParameterization>(),
problem));
if (freeze_landmarks) {
problem->SetParameterBlockConstant(
C_landmarks->at(landmark_id).translation());
problem->SetParameterBlockConstant(
C_landmarks->at(landmark_id).rotation());
}
}
problem->AddResidualBlock(
LandmarkCostFunction2D::CreateAutoDiffCostFunction(
@ -198,6 +209,7 @@ void OptimizationProblem2D::Solve(
MapById<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true;
bool freeze_landmarks = !frozen_trajectories.empty();
for (const auto& submap_id_data : submap_data_) {
const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
@ -231,9 +243,9 @@ void OptimizationProblem2D::Solve(
C_submaps.at(constraint.submap_id).data(),
C_nodes.at(constraint.node_id).data());
}
// Add cost functions for landmarks.
AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
&problem);
// Add cost functions for landmarks.
AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
&C_nodes, &C_landmarks, &problem);
// Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {

4
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -550,8 +550,8 @@ void PoseGraph2D::RunOptimization() {
}
// No other thread is accessing the optimization_problem_, constraints_,
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve
// is time consuming, so not taking the mutex before Solve to avoid blocking
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
// time consuming, so not taking the mutex before Solve to avoid blocking
// foreground processing.
optimization_problem_.Solve(constraints_, frozen_trajectories_,
landmark_nodes_);

3
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -257,6 +257,9 @@ class PoseGraph2D : public PoseGraph {
// Set of all frozen trajectories not being optimized.
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
// Whether or not optimize landmark poses.
bool freeze_landmarks_ GUARDED_BY(mutex_) = false;
// Set of all finished trajectories.
std::set<int> finished_trajectories_ GUARDED_BY(mutex_);

20
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc
View File

@ -117,10 +117,15 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes,
const MapById<NodeId, NodeData>& node_data,
bool freeze_landmarks, const MapById<NodeId, NodeData>& node_data,
MapById<NodeId, CeresPose>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
for (const auto& landmark_node : landmark_nodes) {
// Do not use landmarks that were not optimized for localization.
if (!landmark_node.second.global_landmark_pose.has_value() &&
freeze_landmarks) {
continue;
}
for (const auto& observation : landmark_node.second.landmark_observations) {
const std::string& landmark_id = landmark_node.first;
const auto& begin_of_trajectory =
@ -152,6 +157,12 @@ void AddLandmarkCostFunctions(
CeresPose(starting_point, nullptr /* translation_parametrization */,
common::make_unique<ceres::QuaternionParameterization>(),
problem));
if (freeze_landmarks) {
problem->SetParameterBlockConstant(
C_landmarks->at(landmark_id).translation());
problem->SetParameterBlockConstant(
C_landmarks->at(landmark_id).rotation());
}
}
problem->AddResidualBlock(
LandmarkCostFunction3D::CreateAutoDiffCostFunction(
@ -267,6 +278,7 @@ void OptimizationProblem3D::Solve(
MapById<NodeId, CeresPose> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true;
bool freeze_landmarks = !frozen_trajectories.empty();
for (const auto& submap_id_data : submap_data_) {
const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
@ -325,9 +337,9 @@ void OptimizationProblem3D::Solve(
C_nodes.at(constraint.node_id).rotation(),
C_nodes.at(constraint.node_id).translation());
}
// Add cost functions for landmarks.
AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
&problem);
// Add cost functions for landmarks.
AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
&C_nodes, &C_landmarks, &problem);
// Add constraints based on IMU observations of angular velocities and
// linear acceleration.
if (fix_z_ == FixZ::kNo) {

4
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -593,8 +593,8 @@ void PoseGraph3D::RunOptimization() {
}
// No other thread is accessing the optimization_problem_, constraints_,
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve
// is time consuming, so not taking the mutex before Solve to avoid blocking
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
// time consuming, so not taking the mutex before Solve to avoid blocking
// foreground processing.
optimization_problem_.Solve(constraints_, frozen_trajectories_,
landmark_nodes_);

3
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -261,6 +261,9 @@ class PoseGraph3D : public PoseGraph {
// Set of all frozen trajectories not being optimized.
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
// Whether or not optimize landmark poses.
bool freeze_landmarks_ GUARDED_BY(mutex_) = false;
// Set of all finished trajectories.
std::set<int> finished_trajectories_ GUARDED_BY(mutex_);

6
cartographer/mapping/map_builder.cc
View File

@ -369,7 +369,11 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
sensor::FromProto(
proto.fixed_frame_pose_data().fixed_frame_pose_data()));
}
// TODO(pifon2a, ojura): deserialize landmarks
if (proto.has_landmark_data()) {
pose_graph_->AddLandmarkData(
trajectory_remapping.at(proto.landmark_data().trajectory_id()),
sensor::FromProto(proto.landmark_data().landmark_data()));
}
}
}

6
cartographer/mapping/proto/serialization.proto
View File

@ -48,6 +48,11 @@ message FixedFramePoseData {
sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
}
message LandmarkData {
int32 trajectory_id = 1;
sensor.proto.LandmarkData landmark_data = 2;
}
message TrajectoryData {
int32 trajectory_id = 1;
double gravity_constant = 2;
@ -62,6 +67,7 @@ message SerializedData {
OdometryData odometry_data = 4;
FixedFramePoseData fixed_frame_pose_data = 5;
TrajectoryData trajectory_data = 6;
LandmarkData landmark_data = 7;
}
message LocalSlamResultData {