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

Provide local poses via GetTrajectoryNodePoses() (#1172)

master
Christoph Schütte 2018-05-30 07:34:49 +02:00 committed by GitHub
parent a35092c20f
commit 876a41f06f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 46 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cartographer/cloud/internal/client/pose_graph_stub.cc
View File

@ -89,11 +89,18 @@ PoseGraphStub::GetTrajectoryNodePoses() const {
CHECK(client.Write(request));
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses;
for (const auto& node_pose : client.response().node_poses()) {
node_poses.Insert(mapping::NodeId{node_pose.node_id().trajectory_id(),
node_pose.node_id().node_index()},
mapping::TrajectoryNodePose{
node_pose.has_constant_data(),
transform::ToRigid3(node_pose.global_pose())});
common::optional<mapping::TrajectoryNodePose::ConstantPoseData>
constant_pose_data;
if (node_pose.has_constant_pose_data()) {
constant_pose_data = mapping::TrajectoryNodePose::ConstantPoseData{
common::FromUniversal(node_pose.constant_pose_data().timestamp()),
transform::ToRigid3(node_pose.constant_pose_data().local_pose())};
}
node_poses.Insert(
mapping::NodeId{node_pose.node_id().trajectory_id(),
node_pose.node_id().node_index()},
mapping::TrajectoryNodePose{
transform::ToRigid3(node_pose.global_pose()), constant_pose_data});
}
return node_poses;
}

9
cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.cc
View File

@ -39,7 +39,14 @@ void GetTrajectoryNodePosesHandler::OnRequest(
node_id_pose.id.ToProto(node_pose->mutable_node_id());
*node_pose->mutable_global_pose() =
transform::ToProto(node_id_pose.data.global_pose);
node_pose->set_has_constant_data(node_id_pose.data.has_constant_data);
if (node_id_pose.data.constant_pose_data.has_value()) {
node_pose->mutable_constant_pose_data()->set_timestamp(
common::ToUniversal(
node_id_pose.data.constant_pose_data.value().time));
*node_pose->mutable_constant_pose_data()->mutable_local_pose() =
transform::ToProto(
node_id_pose.data.constant_pose_data.value().local_pose);
}
}
Send(std::move(response));
}

6
cartographer/cloud/proto/map_builder_service.proto
View File

@ -140,9 +140,13 @@ message GetSubmapResponse {
}
message TrajectoryNodePose {
message ConstantPoseData {
int64 timestamp = 1;
cartographer.transform.proto.Rigid3d local_pose = 2;
}
cartographer.mapping.proto.NodeId node_id = 1;
cartographer.transform.proto.Rigid3d global_pose = 2;
bool has_constant_data = 3;
ConstantPoseData constant_pose_data = 3;
}
message GetTrajectoryNodePosesResponse {

9
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -630,10 +630,15 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_);
for (const auto& node_id_data : trajectory_nodes_) {
common::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
if (node_id_data.data.constant_data != nullptr) {
constant_pose_data = TrajectoryNodePose::ConstantPoseData{
node_id_data.data.constant_data->time,
node_id_data.data.constant_data->local_pose};
}
node_poses.Insert(
node_id_data.id,
TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
node_id_data.data.global_pose});
TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data});
}
return node_poses;
}

9
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -671,10 +671,15 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_);
for (const auto& node_id_data : trajectory_nodes_) {
common::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
if (node_id_data.data.constant_data != nullptr) {
constant_pose_data = TrajectoryNodePose::ConstantPoseData{
node_id_data.data.constant_data->time,
node_id_data.data.constant_data->local_pose};
}
node_poses.Insert(
node_id_data.id,
TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
node_id_data.data.global_pose});
TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data});
}
return node_poses;
}

10
cartographer/mapping/trajectory_node.h
View File

@ -21,6 +21,7 @@
#include <vector>
#include "Eigen/Core"
#include "cartographer/common/optional.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory_node_data.pb.h"
#include "cartographer/sensor/range_data.h"
@ -30,11 +31,14 @@ namespace cartographer {
namespace mapping {
struct TrajectoryNodePose {
// Indicates whether the node has constant data set.
bool has_constant_data;
struct ConstantPoseData {
common::Time time;
transform::Rigid3d local_pose;
};
// The node pose in the global SLAM frame.
transform::Rigid3d global_pose;
common::optional<ConstantPoseData> constant_pose_data;
};
struct TrajectoryNode {