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

[GenericPoseGraph] Add a base class for all nodes and Pose2D node. (#1265)

master
Alexander Belyaev 2018-07-12 15:21:59 +02:00 committed by Wally B. Feed
parent 151a78d7be
commit c4c5fab5b2
5 changed files with 261 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cartographer/pose_graph/node.cc Normal file
View File

@ -0,0 +1,31 @@
/*
* 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/pose_graph/node.h"
namespace cartographer {
namespace pose_graph {
proto::Node Node::ToProto() const {
proto::Node node;
node.set_constant(constant_);
node.mutable_id()->set_object_id(node_id_);
*node.mutable_parameters() = ToParametersProto();
return node;
}
} // namespace pose_graph
} // namespace cartographer

83
cartographer/pose_graph/node.h Normal file
View File

@ -0,0 +1,83 @@
/*
* 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.
*/
#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_H_
#include "cartographer/pose_graph/proto/node.pb.h"
#include <string>
#include <vector>
namespace cartographer {
namespace pose_graph {
// TODO(pifon): Change it to struct { string, timestamp} and introduce the
// necessary operators.
using NodeId = std::string;
class Node {
public:
enum class Parameterization {
NONE,
YAW_ONLY,
CONSTANT_YAW,
};
struct ParameterBlock {
// Non-owning pointer to values corresponding to a single parameter block.
double* const values;
// Size of the parameter block.
const size_t size;
const Parameterization parameterization;
};
explicit Node(NodeId id, bool constant) : node_id_(id), constant_(constant) {}
~Node() = default;
Node(const Node&) = delete;
Node& operator=(const Node&) = delete;
proto::Node ToProto() const;
const NodeId node_id() const { return node_id_; }
void set_node_id(const NodeId& id) { node_id_ = id; }
bool constant() const { return constant_; }
void set_constant(bool constant) { constant_ = constant; }
std::vector<ParameterBlock>& parameter_blocks() { return parameter_blocks_; }
protected:
virtual proto::Parameters ToParametersProto() const = 0;
template <std::size_t ArraySize>
void AddParameterBlock(Parameterization parameterization,
std::array<double, ArraySize>* values) {
parameter_blocks_.emplace_back(
ParameterBlock{values->data(), values->size(), parameterization});
}
private:
NodeId node_id_;
bool constant_;
std::vector<ParameterBlock> parameter_blocks_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_H_

48
cartographer/pose_graph/pose_2d.cc Normal file
View File

@ -0,0 +1,48 @@
/*
* 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/pose_graph/pose_2d.h"
namespace cartographer {
namespace pose_graph {
namespace {
constexpr size_t kXIndex = 0;
constexpr size_t kYIndex = 1;
constexpr size_t kRotationIndex = 2;
} // namespace
Pose2D::Pose2D(const NodeId& node_id, bool constant,
const Eigen::Vector2d& translation, double rotation)
: Node(node_id, constant),
pose_2d_{{translation.x(), translation.y(), rotation}} {
AddParameterBlock(Parameterization::NONE, &pose_2d_);
}
proto::Parameters Pose2D::ToParametersProto() const {
proto::Parameters parameters;
auto* pose_2d = parameters.mutable_pose_2d();
pose_2d->set_rotation(pose_2d_[kRotationIndex]);
auto* translation = pose_2d->mutable_translation();
translation->set_x(pose_2d_[kXIndex]);
translation->set_y(pose_2d_[kYIndex]);
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

42
cartographer/pose_graph/pose_2d.h Normal file
View File

@ -0,0 +1,42 @@
/*
* 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.
*/
#ifndef CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_
#define CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_
#include "cartographer/pose_graph/node.h"
#include "Eigen/Core"
namespace cartographer {
namespace pose_graph {
class Pose2D : public Node {
public:
Pose2D(const NodeId& node_id, bool constant,
const Eigen::Vector2d& translation, double rotation);
protected:
proto::Parameters ToParametersProto() const final;
private:
std::array<double, 3> pose_2d_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_

57
cartographer/pose_graph/pose_2d_test.cc Normal file
View File

@ -0,0 +1,57 @@
/*
* 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/pose_graph/pose_2d.h"
#include "gmock/gmock.h"
#include "google/protobuf/text_format.h"
#include "google/protobuf/util/message_differencer.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace pose_graph {
namespace {
constexpr char kExpectedNode[] = R"PROTO(
id { object_id: "flat_world" }
constant: true
parameters {
pose_2d {
translation { x: 1 y: 2 }
rotation: 5
}
}
)PROTO";
// TODO(pifon): Move this to a separate header.
MATCHER_P(EqualsProto, expected_proto_string, "") {
using ConstProtoType = typename std::remove_reference<decltype(arg)>::type;
typename std::remove_cv<ConstProtoType>::type expected_proto;
EXPECT_TRUE(google::protobuf::TextFormat::ParseFromString(
expected_proto_string, &expected_proto));
return google::protobuf::util::MessageDifferencer::Equals(arg,
expected_proto);
}
TEST(Pose2DTest, SerializeToProto) {
Pose2D pose_2d("flat_world", true, Eigen::Vector2d(1., 2.), 5.);
EXPECT_THAT(pose_2d.ToProto(), EqualsProto(kExpectedNode));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer