Change GetTrajectoryNodes() to MapById. (#603)
parent
49ead6055c
commit
7c03467a78
|
@ -153,25 +153,16 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
|
||||||
}
|
}
|
||||||
// Next we serialize all node data.
|
// Next we serialize all node data.
|
||||||
{
|
{
|
||||||
const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
for (const auto& node_id_data : sparse_pose_graph_->GetTrajectoryNodes()) {
|
||||||
for (int trajectory_id = 0;
|
proto::SerializedData proto;
|
||||||
trajectory_id != static_cast<int>(trajectory_nodes.size());
|
auto* const node_proto = proto.mutable_node();
|
||||||
++trajectory_id) {
|
node_proto->mutable_node_id()->set_trajectory_id(
|
||||||
for (int node_index = 0;
|
node_id_data.id.trajectory_id);
|
||||||
node_index !=
|
node_proto->mutable_node_id()->set_node_index(
|
||||||
static_cast<int>(trajectory_nodes[trajectory_id].size());
|
node_id_data.id.node_index);
|
||||||
++node_index) {
|
*node_proto->mutable_node_data() =
|
||||||
proto::SerializedData proto;
|
ToProto(*node_id_data.data.constant_data);
|
||||||
auto* const node_proto = proto.mutable_node();
|
writer->WriteProto(proto);
|
||||||
// TODO(whess): Handle trimmed data.
|
|
||||||
node_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
|
|
||||||
node_proto->mutable_node_id()->set_node_index(node_index);
|
|
||||||
*node_proto->mutable_node_data() =
|
|
||||||
ToProto(*trajectory_nodes[trajectory_id][node_index].constant_data);
|
|
||||||
// TODO(whess): Only enable optionally? Resulting pbstream files will be
|
|
||||||
// a lot larger now.
|
|
||||||
writer->WriteProto(proto);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
// TODO(whess): Serialize additional sensor data: IMU, odometry.
|
// TODO(whess): Serialize additional sensor data: IMU, odometry.
|
||||||
}
|
}
|
||||||
|
@ -206,18 +197,26 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
MapById<NodeId, transform::Rigid3d> node_poses;
|
||||||
|
for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
|
||||||
|
for (const proto::Trajectory::Node& node_proto :
|
||||||
|
trajectory_proto.node()) {
|
||||||
|
node_poses.Insert(NodeId{trajectory_proto.trajectory_id(),
|
||||||
|
node_proto.node_index()},
|
||||||
|
transform::ToRigid3(node_proto.pose()));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
proto::SerializedData proto;
|
proto::SerializedData proto;
|
||||||
if (!reader->ReadProto(&proto)) {
|
if (!reader->ReadProto(&proto)) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (proto.has_node()) {
|
if (proto.has_node()) {
|
||||||
const auto& pose_graph_node =
|
const transform::Rigid3d node_pose =
|
||||||
pose_graph.trajectory(proto.node().node_id().trajectory_id())
|
node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
|
||||||
.node(proto.node().node_id().node_index());
|
proto.node().node_id().node_index()});
|
||||||
const transform::Rigid3d pose =
|
sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, node_pose,
|
||||||
transform::ToRigid3(pose_graph_node.pose());
|
|
||||||
sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, pose,
|
|
||||||
proto.node());
|
proto.node());
|
||||||
}
|
}
|
||||||
if (proto.has_submap()) {
|
if (proto.has_submap()) {
|
||||||
|
|
|
@ -21,8 +21,11 @@ import "cartographer/transform/proto/transform.proto";
|
||||||
option java_outer_classname = "TrajectoryOuterClass";
|
option java_outer_classname = "TrajectoryOuterClass";
|
||||||
|
|
||||||
message Trajectory {
|
message Trajectory {
|
||||||
// NEXT_ID: 7
|
// NEXT_ID: 8
|
||||||
message Node {
|
message Node {
|
||||||
|
// Index of this node within its trajectory.
|
||||||
|
optional int32 node_index = 7;
|
||||||
|
|
||||||
optional int64 timestamp = 1;
|
optional int64 timestamp = 1;
|
||||||
|
|
||||||
// Transform from tracking to global map frame.
|
// Transform from tracking to global map frame.
|
||||||
|
|
|
@ -77,28 +77,15 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
|
||||||
return trajectory_protos.at(trajectory_id);
|
return trajectory_protos.at(trajectory_id);
|
||||||
};
|
};
|
||||||
|
|
||||||
std::map<NodeId, NodeId> node_id_remapping; // Due to trimming.
|
for (const auto& node_id_data : GetTrajectoryNodes()) {
|
||||||
|
CHECK(node_id_data.data.constant_data != nullptr);
|
||||||
const auto all_trajectory_nodes = GetTrajectoryNodes();
|
auto* const node_proto =
|
||||||
for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
|
trajectory(node_id_data.id.trajectory_id)->add_node();
|
||||||
++trajectory_id) {
|
node_proto->set_node_index(node_id_data.id.node_index);
|
||||||
auto* const trajectory_proto = trajectory(trajectory_id);
|
node_proto->set_timestamp(
|
||||||
|
common::ToUniversal(node_id_data.data.constant_data->time));
|
||||||
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
|
*node_proto->mutable_pose() =
|
||||||
for (size_t old_node_index = 0;
|
transform::ToProto(node_id_data.data.global_pose);
|
||||||
old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
|
|
||||||
const auto& node = single_trajectory_nodes[old_node_index];
|
|
||||||
if (node.constant_data != nullptr) {
|
|
||||||
node_id_remapping[NodeId{static_cast<int>(trajectory_id),
|
|
||||||
static_cast<int>(old_node_index)}] =
|
|
||||||
NodeId{static_cast<int>(trajectory_id),
|
|
||||||
static_cast<int>(trajectory_proto->node_size())};
|
|
||||||
auto* node_proto = trajectory_proto->add_node();
|
|
||||||
node_proto->set_timestamp(
|
|
||||||
common::ToUniversal(node.constant_data->time));
|
|
||||||
*node_proto->mutable_pose() = transform::ToProto(node.global_pose);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
for (const auto& submap_id_data : GetAllSubmapData()) {
|
for (const auto& submap_id_data : GetAllSubmapData()) {
|
||||||
|
@ -123,10 +110,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
|
||||||
constraint_proto->mutable_submap_id()->set_submap_index(
|
constraint_proto->mutable_submap_id()->set_submap_index(
|
||||||
constraint.submap_id.submap_index);
|
constraint.submap_id.submap_index);
|
||||||
|
|
||||||
const NodeId node_id = node_id_remapping.at(constraint.node_id);
|
|
||||||
constraint_proto->mutable_node_id()->set_trajectory_id(
|
constraint_proto->mutable_node_id()->set_trajectory_id(
|
||||||
node_id.trajectory_id);
|
constraint.node_id.trajectory_id);
|
||||||
constraint_proto->mutable_node_id()->set_node_index(node_id.node_index);
|
constraint_proto->mutable_node_id()->set_node_index(
|
||||||
|
constraint.node_id.node_index);
|
||||||
|
|
||||||
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
|
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
|
||||||
}
|
}
|
||||||
|
|
|
@ -105,8 +105,7 @@ class SparsePoseGraph {
|
||||||
virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
|
virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
|
||||||
|
|
||||||
// Returns data for all submaps.
|
// Returns data for all submaps.
|
||||||
virtual mapping::MapById<mapping::SubmapId, SubmapData>
|
virtual MapById<SubmapId, SubmapData> GetAllSubmapData() = 0;
|
||||||
GetAllSubmapData() = 0;
|
|
||||||
|
|
||||||
// Returns the transform converting data in the local map frame (i.e. the
|
// Returns the transform converting data in the local map frame (i.e. the
|
||||||
// continuous, non-loop-closed frame) into the global map frame (i.e. the
|
// continuous, non-loop-closed frame) into the global map frame (i.e. the
|
||||||
|
@ -114,7 +113,7 @@ class SparsePoseGraph {
|
||||||
virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;
|
virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;
|
||||||
|
|
||||||
// Returns the current optimized trajectories.
|
// Returns the current optimized trajectories.
|
||||||
virtual std::vector<std::vector<TrajectoryNode>> GetTrajectoryNodes() = 0;
|
virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
|
||||||
|
|
||||||
// Serializes the constraints and trajectories.
|
// Serializes the constraints and trajectories.
|
||||||
proto::SparsePoseGraph ToProto();
|
proto::SparsePoseGraph ToProto();
|
||||||
|
|
|
@ -490,19 +490,10 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
optimized_submap_transforms_ = submap_data;
|
optimized_submap_transforms_ = submap_data;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>>
|
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
|
||||||
SparsePoseGraph::GetTrajectoryNodes() {
|
SparsePoseGraph::GetTrajectoryNodes() {
|
||||||
// TODO(cschuet): Rewrite downstream code and switch to MapById.
|
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>> nodes;
|
return trajectory_nodes_;
|
||||||
for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
|
|
||||||
nodes.resize(trajectory_id + 1);
|
|
||||||
for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
|
|
||||||
nodes.at(trajectory_id).resize(node.id.node_index + 1);
|
|
||||||
nodes.at(trajectory_id).at(node.id.node_index) = node.data;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return nodes;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
|
|
|
@ -98,8 +98,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
GetAllSubmapData() EXCLUDES(mutex_) override;
|
GetAllSubmapData() EXCLUDES(mutex_) override;
|
||||||
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
|
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
|
||||||
EXCLUDES(mutex_) override;
|
EXCLUDES(mutex_) override;
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
|
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
|
||||||
override EXCLUDES(mutex_);
|
GetTrajectoryNodes() override EXCLUDES(mutex_);
|
||||||
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -173,6 +173,11 @@ class SparsePoseGraphTest : public ::testing::Test {
|
||||||
MoveRelativeWithNoise(movement, transform::Rigid2d::Identity());
|
MoveRelativeWithNoise(movement, transform::Rigid2d::Identity());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template <typename Range>
|
||||||
|
std::vector<int> ToVectorInt(const Range& range) {
|
||||||
|
return std::vector<int>(range.begin(), range.end());
|
||||||
|
}
|
||||||
|
|
||||||
sensor::PointCloud point_cloud_;
|
sensor::PointCloud point_cloud_;
|
||||||
std::unique_ptr<ActiveSubmaps> active_submaps_;
|
std::unique_ptr<ActiveSubmaps> active_submaps_;
|
||||||
common::ThreadPool thread_pool_;
|
common::ThreadPool thread_pool_;
|
||||||
|
@ -183,7 +188,7 @@ class SparsePoseGraphTest : public ::testing::Test {
|
||||||
TEST_F(SparsePoseGraphTest, EmptyMap) {
|
TEST_F(SparsePoseGraphTest, EmptyMap) {
|
||||||
sparse_pose_graph_->RunFinalOptimization();
|
sparse_pose_graph_->RunFinalOptimization();
|
||||||
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
||||||
EXPECT_THAT(nodes.size(), ::testing::Eq(0u));
|
EXPECT_TRUE(nodes.empty());
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SparsePoseGraphTest, NoMovement) {
|
TEST_F(SparsePoseGraphTest, NoMovement) {
|
||||||
|
@ -192,13 +197,14 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
|
||||||
MoveRelative(transform::Rigid2d::Identity());
|
MoveRelative(transform::Rigid2d::Identity());
|
||||||
sparse_pose_graph_->RunFinalOptimization();
|
sparse_pose_graph_->RunFinalOptimization();
|
||||||
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
||||||
ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
|
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
|
||||||
EXPECT_THAT(nodes[0].size(), ::testing::Eq(3u));
|
::testing::ContainerEq(std::vector<int>{0}));
|
||||||
EXPECT_THAT(nodes[0][0].global_pose,
|
EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u));
|
||||||
|
EXPECT_THAT(nodes.at(mapping::NodeId{0, 0}).global_pose,
|
||||||
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
||||||
EXPECT_THAT(nodes[0][1].global_pose,
|
EXPECT_THAT(nodes.at(mapping::NodeId{0, 1}).global_pose,
|
||||||
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
||||||
EXPECT_THAT(nodes[0][2].global_pose,
|
EXPECT_THAT(nodes.at(mapping::NodeId{0, 2}).global_pose,
|
||||||
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -212,10 +218,13 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
|
||||||
}
|
}
|
||||||
sparse_pose_graph_->RunFinalOptimization();
|
sparse_pose_graph_->RunFinalOptimization();
|
||||||
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
||||||
ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
|
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
|
||||||
|
::testing::ContainerEq(std::vector<int>{0}));
|
||||||
for (int i = 0; i != 4; ++i) {
|
for (int i = 0; i != 4; ++i) {
|
||||||
EXPECT_THAT(poses[i],
|
EXPECT_THAT(poses[i],
|
||||||
IsNearly(transform::Project2D(nodes[0][i].global_pose), 1e-2))
|
IsNearly(transform::Project2D(
|
||||||
|
nodes.at(mapping::NodeId{0, i}).global_pose),
|
||||||
|
1e-2))
|
||||||
<< i;
|
<< i;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -230,10 +239,13 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) {
|
||||||
}
|
}
|
||||||
sparse_pose_graph_->RunFinalOptimization();
|
sparse_pose_graph_->RunFinalOptimization();
|
||||||
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
||||||
ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
|
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
|
||||||
|
::testing::ContainerEq(std::vector<int>{0}));
|
||||||
for (int i = 0; i != 5; ++i) {
|
for (int i = 0; i != 5; ++i) {
|
||||||
EXPECT_THAT(poses[i],
|
EXPECT_THAT(poses[i],
|
||||||
IsNearly(transform::Project2D(nodes[0][i].global_pose), 1e-2))
|
IsNearly(transform::Project2D(
|
||||||
|
nodes.at(mapping::NodeId{0, i}).global_pose),
|
||||||
|
1e-2))
|
||||||
<< i;
|
<< i;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -255,13 +267,15 @@ TEST_F(SparsePoseGraphTest, OverlappingScans) {
|
||||||
}
|
}
|
||||||
sparse_pose_graph_->RunFinalOptimization();
|
sparse_pose_graph_->RunFinalOptimization();
|
||||||
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
|
||||||
ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
|
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
|
||||||
|
::testing::ContainerEq(std::vector<int>{0}));
|
||||||
transform::Rigid2d true_movement =
|
transform::Rigid2d true_movement =
|
||||||
ground_truth.front().inverse() * ground_truth.back();
|
ground_truth.front().inverse() * ground_truth.back();
|
||||||
transform::Rigid2d movement_before = poses.front().inverse() * poses.back();
|
transform::Rigid2d movement_before = poses.front().inverse() * poses.back();
|
||||||
transform::Rigid2d error_before = movement_before.inverse() * true_movement;
|
transform::Rigid2d error_before = movement_before.inverse() * true_movement;
|
||||||
transform::Rigid3d optimized_movement =
|
transform::Rigid3d optimized_movement =
|
||||||
nodes[0].front().global_pose.inverse() * nodes[0].back().global_pose;
|
nodes.BeginOfTrajectory(0)->data.global_pose.inverse() *
|
||||||
|
std::prev(nodes.EndOfTrajectory(0))->data.global_pose;
|
||||||
transform::Rigid2d optimized_error =
|
transform::Rigid2d optimized_error =
|
||||||
transform::Project2D(optimized_movement).inverse() * true_movement;
|
transform::Project2D(optimized_movement).inverse() * true_movement;
|
||||||
EXPECT_THAT(std::abs(optimized_error.normalized_angle()),
|
EXPECT_THAT(std::abs(optimized_error.normalized_angle()),
|
||||||
|
|
|
@ -531,19 +531,10 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>>
|
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
|
||||||
SparsePoseGraph::GetTrajectoryNodes() {
|
SparsePoseGraph::GetTrajectoryNodes() {
|
||||||
// TODO(cschuet): Rewrite downstream code and switch to MapById.
|
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>> nodes;
|
return trajectory_nodes_;
|
||||||
for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
|
|
||||||
nodes.resize(trajectory_id + 1);
|
|
||||||
for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
|
|
||||||
nodes.at(trajectory_id).resize(node.id.node_index + 1);
|
|
||||||
nodes.at(trajectory_id).at(node.id.node_index) = node.data;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return nodes;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
|
|
|
@ -98,8 +98,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
GetAllSubmapData() EXCLUDES(mutex_) override;
|
GetAllSubmapData() EXCLUDES(mutex_) override;
|
||||||
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
|
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
|
||||||
EXCLUDES(mutex_) override;
|
EXCLUDES(mutex_) override;
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
|
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
|
||||||
override EXCLUDES(mutex_);
|
GetTrajectoryNodes() override EXCLUDES(mutex_);
|
||||||
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
Loading…
Reference in New Issue