diff --git a/cartographer/transform/proto/timestamped_transform.proto b/cartographer/transform/proto/timestamped_transform.proto new file mode 100644 index 0000000..0db6703 --- /dev/null +++ b/cartographer/transform/proto/timestamped_transform.proto @@ -0,0 +1,25 @@ +// Copyright 2018 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. + +syntax = "proto3"; + +package cartographer.transform.proto; + +import "cartographer/transform/proto/transform.proto"; + +// NEXT ID: 3 +message TimestampedTransform { + int64 time = 1; + transform.proto.Rigid3d transform = 2; +} diff --git a/cartographer/transform/timestamped_transform.cc b/cartographer/transform/timestamped_transform.cc index 4901ae2..fd68db0 100644 --- a/cartographer/transform/timestamped_transform.cc +++ b/cartographer/transform/timestamped_transform.cc @@ -16,6 +16,8 @@ #include "cartographer/transform/timestamped_transform.h" +#include "cartographer/transform/transform.h" + namespace cartographer { namespace transform { @@ -36,5 +38,17 @@ TimestampedTransform Interpolate(const TimestampedTransform& start, return TimestampedTransform{time, transform::Rigid3d(origin, rotation)}; } +TimestampedTransform FromProto(const proto::TimestampedTransform& proto) { + return TimestampedTransform{common::FromUniversal(proto.time()), + ToRigid3(proto.transform())}; +} + +proto::TimestampedTransform ToProto(const TimestampedTransform& transform) { + proto::TimestampedTransform proto; + proto.set_time(common::ToUniversal(transform.time)); + *proto.mutable_transform() = ToProto(transform.transform); + return proto; +} + } // namespace transform } // namespace cartographer diff --git a/cartographer/transform/timestamped_transform.h b/cartographer/transform/timestamped_transform.h index 04f2709..dc87082 100644 --- a/cartographer/transform/timestamped_transform.h +++ b/cartographer/transform/timestamped_transform.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_TRANSFORM_TIMESTAMPED_TRANSFORM_H_ #include "cartographer/common/time.h" +#include "cartographer/transform/proto/timestamped_transform.pb.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -28,6 +29,9 @@ struct TimestampedTransform { transform::Rigid3d transform; }; +TimestampedTransform FromProto(const proto::TimestampedTransform& proto); +proto::TimestampedTransform ToProto(const TimestampedTransform& transform); + TimestampedTransform Interpolate(const TimestampedTransform& start, const TimestampedTransform& end, const common::Time time); diff --git a/cartographer/transform/timestamped_transform_test.cc b/cartographer/transform/timestamped_transform_test.cc new file mode 100644 index 0000000..acc5381 --- /dev/null +++ b/cartographer/transform/timestamped_transform_test.cc @@ -0,0 +1,39 @@ +/* + * Copyright 2018 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/transform/timestamped_transform.h" + +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { +namespace { + +TEST(TimestampedTransformTest, ToProtoAndBack) { + const TimestampedTransform expected{ + common::FromUniversal(12345678), + Rigid3d(Eigen::Vector3d(1., 2., 3.), + Eigen::Quaterniond(1., 2., 3., 4.).normalized())}; + const TimestampedTransform actual = FromProto(ToProto(expected)); + EXPECT_EQ(expected.time, actual.time); + EXPECT_THAT(actual.transform, IsNearly(expected.transform, 1e-6)); +} + +} // namespace +} // namespace transform +} // namespace cartographer