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

Adds a 'LandmarkData' struct and proto. (#628)

master
Alexander Belyaev 2017-11-06 15:35:21 +01:00 committed by Wolfgang Hess
parent 77fb50fd76
commit 0d2bc8b938
6 changed files with 228 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
cartographer/sensor/landmark_data.cc Normal file
View File

@ -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

53
cartographer/sensor/landmark_data.h Normal file
View File

@ -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 <string>
#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<Landmark> 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_

75
cartographer/sensor/landmark_data_test.cc Normal file
View File

@ -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 <cmath>
#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<const Landmark&> 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<Landmark> 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

12
cartographer/sensor/proto/sensor.proto
View File

@ -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;
}

9
cartographer/sensor/range_data_test.cc
View File

@ -19,6 +19,7 @@
#include <tuple>
#include <vector>
#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)) {

35
cartographer/sensor/test_helpers.h Normal file
View File

@ -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 <string>
#include <tuple>
#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_