diff --git a/cartographer/sensor/landmark_data.cc b/cartographer/sensor/landmark_data.cc new file mode 100644 index 0000000..453429a --- /dev/null +++ b/cartographer/sensor/landmark_data.cc @@ -0,0 +1,52 @@ +/* + * Copyright 2017 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/sensor/landmark_data.h" + +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +proto::LandmarkData ToProto(const LandmarkData& landmark_data) { + proto::LandmarkData proto; + proto.set_timestamp(common::ToUniversal(landmark_data.time)); + for (const Landmark& landmark : landmark_data.landmarks) { + auto* item = proto.add_landmarks(); + item->set_id(landmark.id); + *item->mutable_transform() = transform::ToProto(landmark.transform); + item->set_translation_weight(landmark.translation_weight); + item->set_rotation_weight(landmark.rotation_weight); + } + return proto; +} + +LandmarkData FromProto(const proto::LandmarkData& proto) { + LandmarkData landmark_data; + landmark_data.time = common::FromUniversal(proto.timestamp()); + for (const auto& item : proto.landmarks()) { + landmark_data.landmarks.push_back({ + item.id(), + transform::ToRigid3(item.transform()), + item.translation_weight(), + item.rotation_weight(), + }); + } + return landmark_data; +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/sensor/landmark_data.h b/cartographer/sensor/landmark_data.h new file mode 100644 index 0000000..ec19ac1 --- /dev/null +++ b/cartographer/sensor/landmark_data.h @@ -0,0 +1,53 @@ +/* + * Copyright 2017 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. + */ + +#ifndef CARTOGRAPHER_SENSOR_LANDMARK_DATA_H_ +#define CARTOGRAPHER_SENSOR_LANDMARK_DATA_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace sensor { + +struct Landmark { + string id; + transform::Rigid3d transform; + double translation_weight; + double rotation_weight; +}; + +struct LandmarkData { + common::Time time; + std::vector landmarks; +}; + +// Converts 'landmark_data' to a proto::LandmarkData. +proto::LandmarkData ToProto(const LandmarkData& landmark_data); + +// Converts 'proto' to an LandmarkData. +LandmarkData FromProto(const proto::LandmarkData& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_LANDMARK_DATA_H_ diff --git a/cartographer/sensor/landmark_data_test.cc b/cartographer/sensor/landmark_data_test.cc new file mode 100644 index 0000000..28bb59e --- /dev/null +++ b/cartographer/sensor/landmark_data_test.cc @@ -0,0 +1,75 @@ +/* + * 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/sensor/landmark_data.h" + +#include + +#include "cartographer/sensor/test_helpers.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::DoubleNear; +using ::testing::Field; + +::testing::Matcher EqualsLandmark(const Landmark& expected) { + return ::testing::AllOf(Field(&Landmark::id, expected.id), + Field(&Landmark::transform, + transform::IsNearly(expected.transform, 1e-2)), + Field(&Landmark::translation_weight, + DoubleNear(expected.translation_weight, 0.01)), + Field(&Landmark::rotation_weight, + DoubleNear(expected.rotation_weight, 0.01))); +} + +class LandmarkDataTest : public ::testing::Test { + protected: + LandmarkDataTest() + : landmarks_( + {{ + "ID1", + transform::Rigid3d(Eigen::Vector3d(1., 1., 1.), + Eigen::Quaterniond(1., 1., -1., -1.)), + 1.f, + 3.f, + }, + { + "ID2", + transform::Rigid3d(Eigen::Vector3d(2., 2., 2.), + Eigen::Quaterniond(2., 2., -2., -2.)), + 2.f, + 4.f, + }}) {} + std::vector landmarks_; +}; + +TEST_F(LandmarkDataTest, LandmarkDataToAndFromProto) { + const auto expected = LandmarkData{common::FromUniversal(50), landmarks_}; + const auto actual = FromProto(ToProto(expected)); + EXPECT_EQ(expected.time, actual.time); + EXPECT_THAT(actual.landmarks, + ElementsAre(EqualsLandmark(expected.landmarks[0]), + EqualsLandmark(expected.landmarks[1]))); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index c51ba33..2f4ddff 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -44,3 +44,15 @@ message FixedFramePoseData { optional int64 timestamp = 1; optional transform.proto.Rigid3d pose = 2; } + +// Proto representation of ::cartographer::sensor::LandmarkData. +message LandmarkData { + message Landmark { + optional bytes id = 1; + optional transform.proto.Rigid3d transform = 2; + optional double translation_weight = 3; + optional double rotation_weight = 4; + } + optional int64 timestamp = 1; + repeated Landmark landmarks = 2; +} diff --git a/cartographer/sensor/range_data_test.cc b/cartographer/sensor/range_data_test.cc index 68cc831..7c495e3 100644 --- a/cartographer/sensor/range_data_test.cc +++ b/cartographer/sensor/range_data_test.cc @@ -19,6 +19,7 @@ #include #include +#include "cartographer/sensor/test_helpers.h" #include "gmock/gmock.h" namespace cartographer { @@ -27,14 +28,6 @@ namespace { using ::testing::Contains; -MATCHER(NearPointwise, std::string(negation ? "Doesn't" : "Does") + " match.") { - return std::get<0>(arg).isApprox(std::get<1>(arg), 0.001f); -} - -MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") { - return arg.isApprox(point, 0.001f); -} - class RangeDataTest : public ::testing::Test { protected: RangeDataTest() : origin_(Eigen::Vector3f(1, 1, 1)) { diff --git a/cartographer/sensor/test_helpers.h b/cartographer/sensor/test_helpers.h new file mode 100644 index 0000000..7e62380 --- /dev/null +++ b/cartographer/sensor/test_helpers.h @@ -0,0 +1,35 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_SENSOR_TEST_HELPERS_H_ +#define CARTOGRAPHER_SENSOR_TEST_HELPERS_H_ + +#include +#include + +#include "gmock/gmock.h" + +namespace cartographer { +namespace sensor { + +MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") { + return arg.isApprox(point, 0.001f); +} + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_TEST_HELPERS_H_