Use landmarks in optimization (both 2D & 3D). (#884)
[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)master
parent
9bebeea742
commit
28993a8963
|
@ -19,6 +19,7 @@
|
|||
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/optional.h"
|
||||
#include "cartographer/mapping/id.h"
|
||||
#include "cartographer/mapping/submaps.h"
|
||||
|
||||
|
@ -58,7 +59,7 @@ class PoseGraphInterface {
|
|||
double rotation_weight;
|
||||
};
|
||||
std::vector<LandmarkObservation> landmark_observations;
|
||||
transform::Rigid3d global_landmark_pose;
|
||||
common::optional<transform::Rigid3d> global_landmark_pose;
|
||||
};
|
||||
|
||||
struct SubmapPose {
|
||||
|
|
|
@ -550,7 +550,8 @@ void PoseGraph::RunOptimization() {
|
|||
// frozen_trajectories_ 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_);
|
||||
optimization_problem_.Solve(constraints_, frozen_trajectories_,
|
||||
landmark_nodes_);
|
||||
common::MutexLocker locker(&mutex_);
|
||||
|
||||
const auto& submap_data = optimization_problem_.submap_data();
|
||||
|
|
|
@ -27,6 +27,8 @@
|
|||
#include "cartographer/common/ceres_solver_options.h"
|
||||
#include "cartographer/common/histogram.h"
|
||||
#include "cartographer/common/math.h"
|
||||
#include "cartographer/mapping/pose_graph/ceres_pose.h"
|
||||
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h"
|
||||
#include "cartographer/mapping_2d/pose_graph/spa_cost_function.h"
|
||||
#include "cartographer/sensor/odometry_data.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
|
@ -39,6 +41,9 @@ namespace pose_graph {
|
|||
|
||||
namespace {
|
||||
|
||||
using ::cartographer::mapping::pose_graph::CeresPose;
|
||||
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
|
||||
|
||||
// Converts a pose into the 3 optimization variable format used for Ceres:
|
||||
// translation in x and y, followed by the rotation angle representing the
|
||||
// orientation.
|
||||
|
@ -52,6 +57,63 @@ transform::Rigid2d ToPose(const std::array<double, 3>& values) {
|
|||
return transform::Rigid2d({values[0], values[1]}, values[2]);
|
||||
}
|
||||
|
||||
// Selects a trajectory node closest in time to the landmark observation and
|
||||
// applies a relative transform from it.
|
||||
transform::Rigid3d GetInitialLandmarkPose(
|
||||
const LandmarkNode::LandmarkObservation& observation,
|
||||
const NodeData& prev_node, const NodeData& next_node) {
|
||||
const NodeData& closest_node =
|
||||
observation.time - prev_node.time < next_node.time - observation.time
|
||||
? prev_node
|
||||
: next_node;
|
||||
return transform::Embed3D(closest_node.pose) *
|
||||
transform::Rigid3d::Rotation(closest_node.gravity_alignment) *
|
||||
observation.landmark_to_tracking_transform;
|
||||
}
|
||||
|
||||
void AddLandmarkCostFunctions(
|
||||
const std::map<std::string, LandmarkNode>& landmark_nodes,
|
||||
const mapping::MapById<mapping::NodeId, NodeData>& node_data,
|
||||
mapping::MapById<mapping::NodeId, std::array<double, 3>>* C_nodes,
|
||||
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
|
||||
for (const auto& landmark_node : landmark_nodes) {
|
||||
for (const auto& observation : landmark_node.second.landmark_observations) {
|
||||
const std::string& landmark_id = landmark_node.first;
|
||||
// Find the trajectory nodes before and after the landmark observation.
|
||||
auto next =
|
||||
node_data.lower_bound(observation.trajectory_id, observation.time);
|
||||
// The landmark observation was made before the trajectory was created.
|
||||
if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) {
|
||||
continue;
|
||||
}
|
||||
// The landmark observation was made, but the next trajectory node has
|
||||
// not been added yet.
|
||||
if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
|
||||
continue;
|
||||
}
|
||||
auto prev = std::prev(next);
|
||||
// Add parameter blocks for the landmark ID if they were not added before.
|
||||
if (!C_landmarks->count(landmark_id)) {
|
||||
const transform::Rigid3d starting_point =
|
||||
landmark_node.second.global_landmark_pose.has_value()
|
||||
? landmark_node.second.global_landmark_pose.value()
|
||||
: GetInitialLandmarkPose(observation, prev->data, next->data);
|
||||
C_landmarks->emplace(
|
||||
landmark_id,
|
||||
CeresPose(starting_point, nullptr /* translation_parametrization */,
|
||||
common::make_unique<ceres::QuaternionParameterization>(),
|
||||
problem));
|
||||
}
|
||||
problem->AddResidualBlock(
|
||||
LandmarkCostFunction::CreateAutoDiffCostFunction(
|
||||
observation, prev->data, next->data),
|
||||
nullptr /* loss function */, C_nodes->at(prev->id).data(),
|
||||
C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(),
|
||||
C_landmarks->at(landmark_id).translation());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
OptimizationProblem::OptimizationProblem(
|
||||
|
@ -112,8 +174,10 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
|
|||
max_num_iterations);
|
||||
}
|
||||
|
||||
void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
||||
const std::set<int>& frozen_trajectories) {
|
||||
void OptimizationProblem::Solve(
|
||||
const std::vector<Constraint>& constraints,
|
||||
const std::set<int>& frozen_trajectories,
|
||||
const std::map<std::string, LandmarkNode>& landmark_nodes) {
|
||||
if (node_data_.empty()) {
|
||||
// Nothing to optimize.
|
||||
return;
|
||||
|
@ -126,6 +190,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
|||
// TODO(hrapp): Move ceres data into SubmapData.
|
||||
mapping::MapById<mapping::SubmapId, std::array<double, 3>> C_submaps;
|
||||
mapping::MapById<mapping::NodeId, std::array<double, 3>> C_nodes;
|
||||
std::map<std::string, CeresPose> C_landmarks;
|
||||
bool first_submap = true;
|
||||
for (const auto& submap_id_data : submap_data_) {
|
||||
const bool frozen =
|
||||
|
@ -160,6 +225,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
|||
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 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();) {
|
||||
|
@ -222,6 +290,11 @@ OptimizationProblem::submap_data() const {
|
|||
return submap_data_;
|
||||
}
|
||||
|
||||
const std::map<std::string, transform::Rigid2d>&
|
||||
OptimizationProblem::landmark_data() const {
|
||||
return landmark_data_;
|
||||
}
|
||||
|
||||
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
|
||||
const {
|
||||
return imu_data_;
|
||||
|
|
|
@ -54,6 +54,7 @@ struct SubmapData {
|
|||
class OptimizationProblem {
|
||||
public:
|
||||
using Constraint = mapping::PoseGraphInterface::Constraint;
|
||||
using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode;
|
||||
|
||||
explicit OptimizationProblem(
|
||||
const mapping::pose_graph::proto::OptimizationProblemOptions& options);
|
||||
|
@ -84,10 +85,12 @@ class OptimizationProblem {
|
|||
|
||||
// Optimizes the global poses.
|
||||
void Solve(const std::vector<Constraint>& constraints,
|
||||
const std::set<int>& frozen_trajectories);
|
||||
const std::set<int>& frozen_trajectories,
|
||||
const std::map<std::string, LandmarkNode>& landmark_nodes);
|
||||
|
||||
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
|
||||
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
|
||||
const std::map<std::string, transform::Rigid2d>& landmark_data() const;
|
||||
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
|
||||
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
|
||||
|
||||
|
@ -102,6 +105,7 @@ class OptimizationProblem {
|
|||
mapping::pose_graph::proto::OptimizationProblemOptions options_;
|
||||
mapping::MapById<mapping::NodeId, NodeData> node_data_;
|
||||
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
|
||||
std::map<std::string, transform::Rigid2d> landmark_data_;
|
||||
sensor::MapByTime<sensor::ImuData> imu_data_;
|
||||
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
||||
};
|
||||
|
|
|
@ -578,7 +578,8 @@ void PoseGraph::RunOptimization() {
|
|||
// No other thread is accessing the optimization_problem_, constraints_ and
|
||||
// frozen_trajectories_ 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_);
|
||||
optimization_problem_.Solve(constraints_, frozen_trajectories_,
|
||||
landmark_nodes_);
|
||||
common::MutexLocker locker(&mutex_);
|
||||
|
||||
const auto& submap_data = optimization_problem_.submap_data();
|
||||
|
|
|
@ -34,6 +34,7 @@
|
|||
#include "cartographer/internal/mapping_3d/rotation_cost_function.h"
|
||||
#include "cartographer/mapping/pose_graph/ceres_pose.h"
|
||||
#include "cartographer/mapping_3d/imu_integration.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/spa_cost_function.h"
|
||||
#include "cartographer/mapping_3d/rotation_parameterization.h"
|
||||
#include "cartographer/transform/timestamped_transform.h"
|
||||
|
@ -49,6 +50,7 @@ namespace pose_graph {
|
|||
namespace {
|
||||
|
||||
using ::cartographer::mapping::pose_graph::CeresPose;
|
||||
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
|
||||
|
||||
// For odometry.
|
||||
std::unique_ptr<transform::Rigid3d> Interpolate(
|
||||
|
@ -98,6 +100,63 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
|
|||
return nullptr;
|
||||
}
|
||||
|
||||
// Selects a trajectory node closest in time to the landmark observation and
|
||||
// applies a relative transform from it.
|
||||
transform::Rigid3d GetInitialLandmarkPose(
|
||||
const LandmarkNode::LandmarkObservation& observation,
|
||||
const NodeData& prev_node, const NodeData& next_node) {
|
||||
const NodeData& closest_node =
|
||||
observation.time - prev_node.time < next_node.time - observation.time
|
||||
? prev_node
|
||||
: next_node;
|
||||
return closest_node.global_pose * observation.landmark_to_tracking_transform;
|
||||
}
|
||||
|
||||
void AddLandmarkCostFunctions(
|
||||
const std::map<std::string, LandmarkNode>& landmark_nodes,
|
||||
const mapping::MapById<mapping::NodeId, NodeData>& node_data,
|
||||
mapping::MapById<mapping::NodeId, CeresPose>* C_nodes,
|
||||
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
|
||||
for (const auto& landmark_node : landmark_nodes) {
|
||||
for (const auto& observation : landmark_node.second.landmark_observations) {
|
||||
const std::string& landmark_id = landmark_node.first;
|
||||
// Find the trajectory nodes before and after the landmark observation.
|
||||
auto next =
|
||||
node_data.lower_bound(observation.trajectory_id, observation.time);
|
||||
// The landmark observation was made before the trajectory was created.
|
||||
if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) {
|
||||
continue;
|
||||
}
|
||||
// The landmark observation was made, but the next trajectory node has
|
||||
// not been added yet.
|
||||
if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
|
||||
continue;
|
||||
}
|
||||
auto prev = std::prev(next);
|
||||
// Add parameter blocks for the landmark ID if they were not added before.
|
||||
if (!C_landmarks->count(landmark_id)) {
|
||||
const transform::Rigid3d starting_point =
|
||||
landmark_node.second.global_landmark_pose.has_value()
|
||||
? landmark_node.second.global_landmark_pose.value()
|
||||
: GetInitialLandmarkPose(observation, prev->data, next->data);
|
||||
C_landmarks->emplace(
|
||||
landmark_id,
|
||||
CeresPose(starting_point, nullptr /* translation_parametrization */,
|
||||
common::make_unique<ceres::QuaternionParameterization>(),
|
||||
problem));
|
||||
}
|
||||
problem->AddResidualBlock(
|
||||
LandmarkCostFunction::CreateAutoDiffCostFunction(
|
||||
observation, prev->data, next->data),
|
||||
nullptr /* loss function */, C_nodes->at(prev->id).rotation(),
|
||||
C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(),
|
||||
C_nodes->at(next->id).translation(),
|
||||
C_landmarks->at(landmark_id).rotation(),
|
||||
C_landmarks->at(landmark_id).translation());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
OptimizationProblem::OptimizationProblem(
|
||||
|
@ -169,8 +228,10 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
|
|||
max_num_iterations);
|
||||
}
|
||||
|
||||
void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
||||
const std::set<int>& frozen_trajectories) {
|
||||
void OptimizationProblem::Solve(
|
||||
const std::vector<Constraint>& constraints,
|
||||
const std::set<int>& frozen_trajectories,
|
||||
const std::map<std::string, LandmarkNode>& landmark_nodes) {
|
||||
if (node_data_.empty()) {
|
||||
// Nothing to optimize.
|
||||
return;
|
||||
|
@ -192,6 +253,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
|||
CHECK(submap_data_.Contains(mapping::SubmapId{0, 0}));
|
||||
mapping::MapById<mapping::SubmapId, CeresPose> C_submaps;
|
||||
mapping::MapById<mapping::NodeId, CeresPose> C_nodes;
|
||||
std::map<std::string, CeresPose> C_landmarks;
|
||||
bool first_submap = true;
|
||||
for (const auto& submap_id_data : submap_data_) {
|
||||
const bool frozen =
|
||||
|
@ -251,6 +313,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
|||
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 constraints based on IMU observations of angular velocities and
|
||||
// linear acceleration.
|
||||
if (fix_z_ == FixZ::kNo) {
|
||||
|
@ -482,6 +547,11 @@ OptimizationProblem::submap_data() const {
|
|||
return submap_data_;
|
||||
}
|
||||
|
||||
const std::map<std::string, transform::Rigid3d>&
|
||||
OptimizationProblem::landmark_data() const {
|
||||
return landmark_data_;
|
||||
}
|
||||
|
||||
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
|
||||
const {
|
||||
return imu_data_;
|
||||
|
|
|
@ -54,6 +54,7 @@ struct SubmapData {
|
|||
class OptimizationProblem {
|
||||
public:
|
||||
using Constraint = mapping::PoseGraphInterface::Constraint;
|
||||
using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode;
|
||||
|
||||
enum class FixZ { kYes, kNo };
|
||||
|
||||
|
@ -88,10 +89,12 @@ class OptimizationProblem {
|
|||
|
||||
// Optimizes the global poses.
|
||||
void Solve(const std::vector<Constraint>& constraints,
|
||||
const std::set<int>& frozen_trajectories);
|
||||
const std::set<int>& frozen_trajectories,
|
||||
const std::map<std::string, LandmarkNode>& landmark_nodes);
|
||||
|
||||
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
|
||||
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
|
||||
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
|
||||
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
|
||||
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
|
||||
const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
|
||||
|
@ -113,6 +116,7 @@ class OptimizationProblem {
|
|||
FixZ fix_z_;
|
||||
mapping::MapById<mapping::NodeId, NodeData> node_data_;
|
||||
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
|
||||
std::map<std::string, transform::Rigid3d> landmark_data_;
|
||||
sensor::MapByTime<sensor::ImuData> imu_data_;
|
||||
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
||||
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
|
||||
|
|
|
@ -170,7 +170,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
|
|||
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
|
||||
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
|
||||
const std::set<int> kFrozen;
|
||||
optimization_problem_.Solve(constraints, kFrozen);
|
||||
optimization_problem_.Solve(constraints, kFrozen, {});
|
||||
|
||||
double translation_error_after = 0.;
|
||||
double rotation_error_after = 0.;
|
||||
|
|
Loading…
Reference in New Issue