Unify 2D/3D constraints. (#119)

This removes Constraint2D and instead embeds 2D constraints in a
Constraint3D which is renamed to Constraint.
master
Wolfgang Hess 2016-11-08 14:47:03 +01:00 committed by GitHub
parent 3e64a803a4
commit c80c2eaa40
15 changed files with 63 additions and 110 deletions

View File

@ -28,22 +28,11 @@ namespace cartographer {
namespace mapping {
proto::SparsePoseGraph::Constraint::Tag ToProto(
const SparsePoseGraph::Constraint2D::Tag& tag) {
const SparsePoseGraph::Constraint::Tag& tag) {
switch (tag) {
case SparsePoseGraph::Constraint2D::Tag::INTRA_SUBMAP:
case SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP:
return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP;
case SparsePoseGraph::Constraint2D::Tag::INTER_SUBMAP:
return proto::SparsePoseGraph::Constraint::INTER_SUBMAP;
}
LOG(FATAL) << "Unsupported tag.";
}
proto::SparsePoseGraph::Constraint::Tag ToProto(
const SparsePoseGraph::Constraint3D::Tag& tag) {
switch (tag) {
case SparsePoseGraph::Constraint3D::Tag::INTRA_SUBMAP:
return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP;
case SparsePoseGraph::Constraint3D::Tag::INTER_SUBMAP:
case SparsePoseGraph::Constraint::Tag::INTER_SUBMAP:
return proto::SparsePoseGraph::Constraint::INTER_SUBMAP;
}
LOG(FATAL) << "Unsupported tag.";
@ -85,26 +74,7 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;
for (const auto& constraint : constraints_2d()) {
auto* const constraint_proto = proto.add_constraint();
*constraint_proto->mutable_relative_pose() =
transform::ToProto(transform::Embed3D(constraint.pose.zbar_ij));
for (int i = 0; i != 36; ++i) {
constraint_proto->mutable_sqrt_lambda()->Add(0.);
}
constexpr double kFakePositionCovariance = 1.;
constexpr double kFakeOrientationCovariance = 1.;
Eigen::Map<Eigen::Matrix<double, 6, 6>>(
constraint_proto->mutable_sqrt_lambda()->mutable_data()) =
kalman_filter::Embed3D(constraint.pose.sqrt_Lambda_ij,
kFakePositionCovariance,
kFakeOrientationCovariance);
// TODO(whess): Support multi-trajectory.
constraint_proto->mutable_submap_id()->set_submap_index(constraint.i);
constraint_proto->mutable_scan_id()->set_scan_index(constraint.j);
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
}
for (const auto& constraint : constraints_3d()) {
for (const auto& constraint : constraints()) {
auto* const constraint_proto = proto.add_constraint();
*constraint_proto->mutable_relative_pose() =
transform::ToProto(constraint.pose.zbar_ij);

View File

@ -42,28 +42,7 @@ class SparsePoseGraph {
// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
// pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
// 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
struct Constraint2D {
struct Pose {
transform::Rigid2d zbar_ij;
Eigen::Matrix<double, 3, 3> sqrt_Lambda_ij;
};
// Submap index.
int i;
// Scan index.
int j;
// Pose of the scan 'j' relative to submap 'i'.
Pose pose;
// Differentiates between intra-submap (where scan 'j' was inserted into
// submap 'i') and inter-submap constraints (where scan 'j' was not inserted
// into submap 'i').
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};
struct Constraint3D {
struct Constraint {
struct Pose {
transform::Rigid3d zbar_ij;
Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
@ -116,11 +95,8 @@ class SparsePoseGraph {
// Returns the current optimized trajectory.
virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
// Returns the collection of 2D constraints.
virtual std::vector<Constraint2D> constraints_2d() = 0;
// Returns the collection of 3D constraints.
virtual std::vector<Constraint3D> constraints_3d() = 0;
// Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0;
// Serializes the constraints and the computed trajectory.
//

View File

@ -172,14 +172,18 @@ void SparsePoseGraph::ComputeConstraintsForScan(
// Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
constraints_.push_back(Constraint2D{
constexpr double kFakePositionCovariance = 1.;
constexpr double kFakeOrientationCovariance = 1.;
constraints_.push_back(Constraint{
submap_index,
scan_index,
{constraint_transform,
{transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint2D::INTRA_SUBMAP});
kalman_filter::Embed3D(covariance, kFakePositionCovariance,
kFakeOrientationCovariance),
options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint::INTRA_SUBMAP});
}
// Determine if this scan should be globally localized.
@ -389,14 +393,10 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_;
}
std::vector<SparsePoseGraph::Constraint2D> SparsePoseGraph::constraints_2d() {
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return constraints_;
}
std::vector<SparsePoseGraph::Constraint3D> SparsePoseGraph::constraints_3d() {
return {};
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps& submaps) {
return GetSubmapTransforms(submaps).back() *

View File

@ -90,8 +90,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
std::vector<Constraint2D> constraints_2d() override;
std::vector<Constraint3D> constraints_3d() override;
std::vector<Constraint> constraints() override;
private:
struct SubmapState {
@ -179,7 +178,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current optimization problem.
sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint2D> constraints_;
std::vector<Constraint> constraints_;
std::vector<transform::Rigid2d> initial_point_cloud_poses_;
std::vector<transform::Rigid2d> point_cloud_poses_; // (map <- point cloud)
std::vector<transform::Rigid2d> submap_transforms_; // (map <- submap)

View File

@ -31,10 +31,10 @@ google_library(mapping_2d_sparse_pose_graph_constraint_builder
mapping_2d_scan_matching_fast_correlative_scan_matcher
mapping_2d_scan_matching_proto_ceres_scan_matcher_options
mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options
mapping_2d_sparse_pose_graph_optimization_problem
mapping_2d_submaps
mapping_3d_scan_matching_ceres_scan_matcher
mapping_3d_scan_matching_fast_correlative_scan_matcher
mapping_sparse_pose_graph
mapping_sparse_pose_graph_proto_constraint_builder_options
mapping_trajectory_connectivity
sensor_point_cloud
@ -70,6 +70,8 @@ google_library(mapping_2d_sparse_pose_graph_spa_cost_function
spa_cost_function.h
DEPENDS
common_math
kalman_filter_pose_tracker
mapping_sparse_pose_graph
transform_rigid_transform
transform_transform
)

View File

@ -176,7 +176,7 @@ void ConstraintBuilder::ComputeConstraint(
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
const SubmapScanMatcher* const submap_scan_matcher =
@ -226,13 +226,17 @@ void ConstraintBuilder::ComputeConstraint(
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
constraint->reset(new OptimizationProblem::Constraint{
constexpr double kFakePositionCovariance = 1.;
constexpr double kFakeOrientationCovariance = 1.;
constraint->reset(new Constraint{
submap_index,
scan_index,
{constraint_transform,
{transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.lower_covariance_eigenvalue_bound())},
OptimizationProblem::Constraint::INTER_SUBMAP});
kalman_filter::Embed3D(covariance, kFakePositionCovariance,
kFakeOrientationCovariance),
options_.lower_covariance_eigenvalue_bound())},
Constraint::INTER_SUBMAP});
if (options_.log_matches()) {
const transform::Rigid2d difference =
@ -261,8 +265,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
if (pending_computations_.empty()) {
CHECK_EQ(submap_queued_work_items_.size(), 0);
if (when_done_ != nullptr) {
for (const std::unique_ptr<OptimizationProblem::Constraint>&
constraint : constraints_) {
for (const std::unique_ptr<Constraint>& constraint : constraints_) {
if (constraint != nullptr) {
result.push_back(*constraint);
}

View File

@ -30,11 +30,11 @@
#include "cartographer/common/math.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
@ -59,7 +59,7 @@ transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap);
// This class is thread-safe.
class ConstraintBuilder {
public:
using Constraint = mapping::SparsePoseGraph::Constraint2D;
using Constraint = mapping::SparsePoseGraph::Constraint;
using Result = std::vector<Constraint>;
ConstraintBuilder(

View File

@ -134,12 +134,17 @@ void OptimizationProblem::Solve(
// This pose has a predecessor.
if (last_pose_indices.count(trajectory) != 0) {
const int last_pose_index = last_pose_indices[trajectory];
constexpr double kUnusedPositionPenalty = 1.;
constexpr double kUnusedOrientationPenalty = 1.;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
initial_point_cloud_poses[last_pose_index].inverse() *
initial_point_cloud_poses[j],
consecutive_pose_change_penalty_matrix})),
transform::Embed3D(
initial_point_cloud_poses[last_pose_index].inverse() *
initial_point_cloud_poses[j]),
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
kUnusedPositionPenalty,
kUnusedOrientationPenalty)})),
nullptr /* loss function */, C_point_clouds[last_pose_index].data(),
C_point_clouds[j].data());
}

View File

@ -36,7 +36,7 @@ namespace sparse_pose_graph {
// Implements the SPA loop closure method.
class OptimizationProblem {
public:
using Constraint = mapping::SparsePoseGraph::Constraint2D;
using Constraint = mapping::SparsePoseGraph::Constraint;
explicit OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&

View File

@ -22,8 +22,10 @@
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/common/math.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "ceres/jet.h"
@ -33,7 +35,7 @@ namespace sparse_pose_graph {
class SpaCostFunction {
public:
using Constraint = mapping::SparsePoseGraph::Constraint2D;
using Constraint = mapping::SparsePoseGraph::Constraint;
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
@ -62,9 +64,10 @@ class SpaCostFunction {
static void ComputeScaledError(const Constraint::Pose& pose,
const T* const c_i, const T* const c_j,
T* const e) {
std::array<T, 3> e_ij = ComputeUnscaledError(pose.zbar_ij, c_i, c_j);
std::array<T, 3> e_ij =
ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j);
(Eigen::Map<Eigen::Matrix<T, 3, 1>>(e)) =
pose.sqrt_Lambda_ij.cast<T>() *
kalman_filter::Project2D(pose.sqrt_Lambda_ij).cast<T>() *
Eigen::Map<Eigen::Matrix<T, 3, 1>>(e_ij.data());
}

View File

@ -184,14 +184,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
// Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose;
constraints_.push_back(Constraint3D{
submap_index,
scan_index,
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint3D::INTRA_SUBMAP});
constraints_.push_back(
Constraint{submap_index,
scan_index,
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint::INTRA_SUBMAP});
}
// Determine if this scan should be globally localized.
@ -393,11 +393,7 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_;
}
std::vector<SparsePoseGraph::Constraint2D> SparsePoseGraph::constraints_2d() {
return {};
}
std::vector<SparsePoseGraph::Constraint3D> SparsePoseGraph::constraints_3d() {
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return constraints_;
}

View File

@ -92,8 +92,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
std::vector<Constraint2D> constraints_2d() override;
std::vector<Constraint3D> constraints_3d() override;
std::vector<Constraint> constraints() override;
private:
struct SubmapState {
@ -179,7 +178,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current optimization problem.
sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint3D> constraints_;
std::vector<Constraint> constraints_;
std::vector<transform::Rigid3d> submap_transforms_; // (map <- submap)
// Submaps get assigned an index and state as soon as they are seen, even

View File

@ -55,7 +55,7 @@ namespace sparse_pose_graph {
// This class is thread-safe.
class ConstraintBuilder {
public:
using Constraint = mapping::SparsePoseGraph::Constraint3D;
using Constraint = mapping::SparsePoseGraph::Constraint;
using Result = std::vector<Constraint>;
ConstraintBuilder(

View File

@ -44,7 +44,7 @@ struct NodeData {
// Implements the SPA loop closure method.
class OptimizationProblem {
public:
using Constraint = mapping::SparsePoseGraph::Constraint3D;
using Constraint = mapping::SparsePoseGraph::Constraint;
explicit OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&

View File

@ -33,7 +33,7 @@ namespace sparse_pose_graph {
class SpaCostFunction {
public:
using Constraint = mapping::SparsePoseGraph::Constraint3D;
using Constraint = mapping::SparsePoseGraph::Constraint;
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}