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

Store node initial pose in constant data. (#568)

master
Juraj Oršulić 2017-10-05 16:10:12 +02:00 committed by Wolfgang Hess
parent cae9c02e05
commit bd8a2e6a92
14 changed files with 71 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping/global_trajectory_builder.h
View File

@ -51,9 +51,8 @@ class GlobalTrajectoryBuilder
if (insertion_result == nullptr) {
return;
}
sparse_pose_graph_->AddScan(
insertion_result->constant_data, insertion_result->pose_observation,
trajectory_id_, insertion_result->insertion_submaps);
sparse_pose_graph_->AddScan(insertion_result->constant_data, trajectory_id_,
insertion_result->insertion_submaps);
}
void AddSensorData(const sensor::ImuData& imu_data) override {

1
cartographer/mapping/proto/trajectory_node_data.proto
View File

@ -28,4 +28,5 @@ message TrajectoryNodeData {
optional sensor.proto.CompressedPointCloud high_resolution_point_cloud = 4;
optional sensor.proto.CompressedPointCloud low_resolution_point_cloud = 5;
repeated float rotational_scan_matcher_histogram = 6;
optional transform.proto.Rigid3d initial_pose = 7;
}

5
cartographer/mapping/trajectory_node.cc
View File

@ -44,6 +44,8 @@ proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) {
proto.add_rotational_scan_matcher_histogram(
constant_data.rotational_scan_matcher_histogram(i));
}
*proto.mutable_initial_pose() =
transform::ToProto(constant_data.initial_pose);
return proto;
}
@ -63,7 +65,8 @@ TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto) {
.Decompress(),
sensor::CompressedPointCloud(proto.low_resolution_point_cloud())
.Decompress(),
rotational_scan_matcher_histogram};
rotational_scan_matcher_histogram,
transform::ToRigid3(proto.initial_pose())};
}
} // namespace mapping

3
cartographer/mapping/trajectory_node.h
View File

@ -45,6 +45,9 @@ struct TrajectoryNode {
sensor::PointCloud high_resolution_point_cloud;
sensor::PointCloud low_resolution_point_cloud;
Eigen::VectorXf rotational_scan_matcher_histogram;
// The initial unoptimized node pose.
transform::Rigid3d initial_pose;
};
common::Time time() const { return constant_data->time; }

6
cartographer/mapping/trajectory_node_test.cc
View File

@ -21,6 +21,7 @@
#include "Eigen/Core"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory_node_data.pb.h"
#include "cartographer/transform/rigid_transform_test_helpers.h"
#include "gtest/gtest.h"
namespace cartographer {
@ -36,7 +37,8 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
sensor::CompressedPointCloud({{2.f, 3.f, 4.f}}).Decompress(),
sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(),
Eigen::VectorXf::Unit(20, 4),
};
transform::Rigid3d({1., 2., 3.},
Eigen::Quaterniond(4., 5., -6., -7.).normalized())};
const proto::TrajectoryNodeData proto = ToProto(expected);
const TrajectoryNode::Data actual = FromProto(proto);
EXPECT_EQ(expected.time, actual.time);
@ -49,6 +51,8 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
actual.low_resolution_point_cloud);
EXPECT_EQ(expected.rotational_scan_matcher_histogram,
actual.rotational_scan_matcher_histogram);
EXPECT_THAT(actual.initial_pose,
transform::IsNearly(expected.initial_pose, 1e-9));
}
} // namespace

5
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -184,8 +184,9 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
filtered_gravity_aligned_point_cloud,
{}, // 'high_resolution_point_cloud' is only used in 3D.
{}, // 'low_resolution_point_cloud' is only used in 3D.
}),
pose_estimate, std::move(insertion_submaps)});
{}, // 'rotational_scan_matcher_histogram' is only used in 3D.
pose_estimate}),
std::move(insertion_submaps)});
}
const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {

1
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -42,7 +42,6 @@ class LocalTrajectoryBuilder {
public:
struct InsertionResult {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
transform::Rigid3d pose_observation;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
};

48
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -96,10 +96,10 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid3d& pose, const int trajectory_id,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose);
GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append(
@ -132,11 +132,8 @@ void SparsePoseGraph::AddScan(
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(
trajectory_id, insertion_submaps, newly_finished_submap,
transform::Project2D(pose *
transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse())));
ComputeConstraintsForScan(trajectory_id, insertion_submaps,
newly_finished_submap);
});
}
@ -227,11 +224,27 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
void SparsePoseGraph::ComputeConstraintsForScan(
const int trajectory_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap, const transform::Rigid2d& pose) {
const bool newly_finished_submap) {
const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front();
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const transform::Rigid2d pose = transform::Project2D(
constant_data->initial_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data()
.at(matching_id.trajectory_id)
@ -240,22 +253,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() *
pose;
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
const auto& node_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
node_data->time, pose, optimized_pose,
node_data->gravity_alignment);
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
constant_data->gravity_alignment);
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[i];
// Even if this was the last scan added to 'submap_id', the submap will only

5
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -71,7 +71,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// 'true', this submap was inserted into for the last time.
void AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid3d& pose, int trajectory_id,
int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_);
@ -130,8 +130,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void ComputeConstraintsForScan(
int trajectory_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
bool newly_finished_submap, const transform::Rigid2d& pose)
REQUIRES(mutex_);
bool newly_finished_submap) REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
void ComputeConstraint(const mapping::NodeId& node_id,

5
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -164,8 +164,9 @@ class SparsePoseGraphTest : public ::testing::Test {
range_data.returns,
{},
{},
{}}),
transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps);
{},
transform::Embed3D(pose_estimate)}),
kTrajectoryId, insertion_submaps);
}
void MoveRelative(const transform::Rigid2d& movement) {

5
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -221,8 +221,9 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
{}, // 'filtered_point_cloud' is only used in 2D.
high_resolution_point_cloud,
low_resolution_point_cloud,
rotational_scan_matcher_histogram}),
pose_observation, std::move(insertion_submaps)});
rotational_scan_matcher_histogram,
pose_observation}),
std::move(insertion_submaps)});
}
} // namespace mapping_3d

1
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -42,7 +42,6 @@ class LocalTrajectoryBuilder {
public:
struct InsertionResult {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
transform::Rigid3d pose_observation;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
};

37
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -94,10 +94,10 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid3d& pose, const int trajectory_id,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose);
GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append(
@ -131,7 +131,7 @@ void SparsePoseGraph::AddScan(
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(trajectory_id, insertion_submaps,
newly_finished_submap, pose);
newly_finished_submap);
});
}
@ -250,32 +250,33 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
void SparsePoseGraph::ComputeConstraintsForScan(
const int trajectory_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap, const transform::Rigid3d& pose) {
const bool newly_finished_submap) {
const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front();
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const transform::Rigid3d& pose = constant_data->initial_pose;
const transform::Rigid3d optimized_pose =
optimization_problem_.submap_data()
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
insertion_submaps.front()->local_pose().inverse() * pose;
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
matching_id.trajectory_id, constant_data->time, pose, optimized_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[i];
// Even if this was the last scan added to 'submap_id', the submap will only

5
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -72,7 +72,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// 'true', this submap was inserted into for the last time.
void AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const transform::Rigid3d& pose, int trajectory_id,
int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_);
@ -131,8 +131,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void ComputeConstraintsForScan(
int trajectory_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
bool newly_finished_submap, const transform::Rigid3d& pose)
REQUIRES(mutex_);
bool newly_finished_submap) REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
void ComputeConstraint(const mapping::NodeId& node_id,