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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
cartographer/mapping/sparse_pose_graph.cc
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);

30
cartographer/mapping/sparse_pose_graph.h
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.
//

20
cartographer/mapping_2d/sparse_pose_graph.cc
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() *

5
cartographer/mapping_2d/sparse_pose_graph.h
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)

4
cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt
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
)

17
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
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);
}

4
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
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(

11
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
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());
}

2
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
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&

9
cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h
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());
}

22
cartographer/mapping_3d/sparse_pose_graph.cc
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_;
}

5
cartographer/mapping_3d/sparse_pose_graph.h
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

2
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
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(

2
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
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&

2
cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h
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) {}