Add code to serialize a trajectory to proto. (#87)

master
Holger Rapp 2016-10-20 10:53:43 +02:00 committed by Wolfgang Hess
parent f2916143ef
commit 4b7e2efbd9
4 changed files with 47 additions and 2 deletions

View File

@ -171,12 +171,16 @@ google_library(mapping_trajectory_connectivity
google_library(mapping_trajectory_node google_library(mapping_trajectory_node
USES_EIGEN USES_EIGEN
SRCS
trajectory_node.cc
HDRS HDRS
trajectory_node.h trajectory_node.h
DEPENDS DEPENDS
common_time common_time
proto_trajectory
sensor_laser sensor_laser
transform_rigid_transform transform_rigid_transform
transform_transform
) )
google_test(mapping_probability_values_test google_test(mapping_probability_values_test

View File

@ -0,0 +1,39 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/common/time.h"
#include "cartographer/proto/trajectory.pb.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace mapping {
proto::Trajectory ToProto(const std::vector<TrajectoryNode>& nodes) {
proto::Trajectory trajectory;
for (const auto& node : nodes) {
const auto& data = *node.constant_data;
auto* node_proto = trajectory.add_node();
node_proto->set_timestamp(common::ToUniversal(data.time));
*node_proto->mutable_pose() =
transform::ToProto(node.pose * data.tracking_to_pose);
}
return trajectory;
}
} // namespace mapping
} // namespace cartographer

View File

@ -22,6 +22,7 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/proto/trajectory.pb.h"
#include "cartographer/sensor/laser.h" #include "cartographer/sensor/laser.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -66,6 +67,9 @@ struct TrajectoryNodes {
std::vector<mapping::TrajectoryNode> trajectory_nodes; std::vector<mapping::TrajectoryNode> trajectory_nodes;
}; };
::cartographer::proto::Trajectory ToProto(
const std::vector<TrajectoryNode>& nodes);
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -24,8 +24,6 @@ message Trajectory {
// NEXT_ID: 7 // NEXT_ID: 7
message Node { message Node {
optional int64 timestamp = 1; optional int64 timestamp = 1;
// 2D Pose in map frame.
optional transform.proto.Rigid2d pose_2d = 2;
// Transform from tracking to map frame. // Transform from tracking to map frame.
optional transform.proto.Rigid3d pose = 5; optional transform.proto.Rigid3d pose = 5;
} }