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

[GenericPoseGraph] Add PoseGraphController. (#1335)

master
Alexander Belyaev 2018-07-27 20:35:27 +02:00 committed by GitHub
parent 5b44305ea3
commit 3c5f2cd154
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 244 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer/pose_graph/node/imu_calibration.cc
View File

@ -20,12 +20,13 @@ namespace cartographer {
namespace pose_graph {
ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant,
double gravity_constant,
const Eigen::Quaterniond& orientation)
const proto::ImuCalibration& imu_calibration)
: Node(node_id, constant),
gravity_constant_(gravity_constant),
orientation_{{orientation.x(), orientation.y(), orientation.z(),
orientation.w()}} {}
gravity_constant_(imu_calibration.gravity_constant()),
orientation_{{imu_calibration.orientation().x(),
imu_calibration.orientation().y(),
imu_calibration.orientation().z(),
imu_calibration.orientation().w()}} {}
proto::Parameters ImuCalibration::ToParametersProto() const {
proto::Parameters parameters;

4
cartographer/pose_graph/node/imu_calibration.h
View File

@ -28,8 +28,8 @@ namespace pose_graph {
class ImuCalibration : public Node {
public:
ImuCalibration(const NodeId& node_id, bool constant, double gravity_constant,
const Eigen::Quaterniond& orientation);
ImuCalibration(const NodeId& node_id, bool constant,
const proto::ImuCalibration& imu_calibration);
double* mutable_gravity_constant() { return &gravity_constant_; }
double gravity_constant() const { return gravity_constant_; }

7
cartographer/pose_graph/node/imu_calibration_test.cc
View File

@ -22,6 +22,8 @@ namespace cartographer {
namespace pose_graph {
namespace {
using testing::ParseProto;
constexpr char kExpectedNode[] = R"PROTO(
id { object_id: "accelerometer" timestamp: 1 }
constant: true
@ -35,7 +37,10 @@ constexpr char kExpectedNode[] = R"PROTO(
TEST(Pose3DTest, ToProto) {
ImuCalibration imu_calibration({"accelerometer", common::FromUniversal(1)},
true, 10, Eigen::Quaterniond(0., 1., 2., 3.));
true, ParseProto<proto::ImuCalibration>(R"(
gravity_constant: 10
orientation: { w: 0 x: 1 y: 2 z: 3 }
)"));
EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode));
}

5
cartographer/pose_graph/node/pose_2d.cc
View File

@ -27,9 +27,10 @@ constexpr size_t kRotationIndex = 2;
} // namespace
Pose2D::Pose2D(const NodeId& node_id, bool constant,
const Eigen::Vector2d& translation, double rotation)
const proto::Pose2D& pose_2d)
: Node(node_id, constant),
pose_2d_{{translation.x(), translation.y(), rotation}} {}
pose_2d_{{pose_2d.translation().x(), pose_2d.translation().y(),
pose_2d.rotation()}} {}
proto::Parameters Pose2D::ToParametersProto() const {
proto::Parameters parameters;

3
cartographer/pose_graph/node/pose_2d.h
View File

@ -28,8 +28,7 @@ namespace pose_graph {
class Pose2D : public Node {
public:
Pose2D(const NodeId& node_id, bool constant,
const Eigen::Vector2d& translation, double rotation);
Pose2D(const NodeId& node_id, bool constant, const proto::Pose2D& pose_2d);
std::array<double, 3>* mutable_pose_2d() { return &pose_2d_; }
const std::array<double, 3>& pose_2d() const { return pose_2d_; }

7
cartographer/pose_graph/node/pose_2d_test.cc
View File

@ -22,6 +22,8 @@ namespace cartographer {
namespace pose_graph {
namespace {
using testing::ParseProto;
constexpr char kExpectedNode[] = R"PROTO(
id { object_id: "flat_world" timestamp: 1 }
constant: true
@ -35,7 +37,10 @@ constexpr char kExpectedNode[] = R"PROTO(
TEST(Pose2DTest, ToProto) {
Pose2D pose_2d({"flat_world", common::FromUniversal(1)}, true,
Eigen::Vector2d(1., 2.), 5.);
ParseProto<proto::Pose2D>(R"(
translation { x: 1 y: 2 }
rotation: 5
)"));
EXPECT_THAT(pose_2d.ToProto(), testing::EqualsProto(kExpectedNode));
}

9
cartographer/pose_graph/node/pose_3d.cc
View File

@ -20,11 +20,12 @@ namespace cartographer {
namespace pose_graph {
Pose3D::Pose3D(const NodeId& node_id, bool constant,
const Eigen::Vector3d& translation,
const Eigen::Quaterniond& rotation)
const proto::Pose3D& pose_3d)
: Node(node_id, constant),
translation_{{translation.x(), translation.y(), translation.z()}},
rotation_{{rotation.x(), rotation.y(), rotation.z(), rotation.w()}} {}
translation_{{pose_3d.translation().x(), pose_3d.translation().y(),
pose_3d.translation().z()}},
rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(),
pose_3d.rotation().z(), pose_3d.rotation().w()}} {}
proto::Parameters Pose3D::ToParametersProto() const {
proto::Parameters parameters;

4
cartographer/pose_graph/node/pose_3d.h
View File

@ -28,9 +28,7 @@ namespace pose_graph {
class Pose3D : public Node {
public:
Pose3D(const NodeId& node_id, bool constant,
const Eigen::Vector3d& translation,
const Eigen::Quaterniond& rotation);
Pose3D(const NodeId& node_id, bool constant, const proto::Pose3D& pose_3d);
std::array<double, 3>* mutable_translation() { return &translation_; }
const std::array<double, 3>& translation() const { return translation_; }

8
cartographer/pose_graph/node/pose_3d_test.cc
View File

@ -22,6 +22,8 @@ namespace cartographer {
namespace pose_graph {
namespace {
using testing::ParseProto;
constexpr char kExpectedNode[] = R"PROTO(
id { object_id: "bumpy_world" timestamp: 1 }
constant: true
@ -35,8 +37,10 @@ constexpr char kExpectedNode[] = R"PROTO(
TEST(Pose3DTest, ToProto) {
Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true,
Eigen::Vector3d(1., 2., 3.),
Eigen::Quaterniond(0., 1., 2., 3.));
ParseProto<proto::Pose3D>(R"(
translation { x: 1 y: 2 z: 3 }
rotation: { w: 0 x: 1 y: 2 z: 3 }
)"));
EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode));
}

27
cartographer/pose_graph/optimizer/ceres_optimizer_test.cc
View File

@ -26,13 +26,6 @@ namespace {
using testing::ParseProto;
// TODO(pifon): Use the factory function, when the factory is done.
Pose2D GetPose2D(const proto::Node& proto) {
return {NodeId(proto.id()), proto.constant(),
transform::ToEigen(proto.parameters().pose_2d().translation()),
proto.parameters().pose_2d().rotation()};
}
constexpr char kStartNode[] = R"PROTO(
id { object_id: "start_node" timestamp: 1 }
constant: false
@ -56,6 +49,10 @@ constexpr char kEndNode[] = R"PROTO(
)PROTO";
constexpr char kRelativePose2D[] = R"PROTO(
id: "constraint_1"
loss_function { quadratic_loss: {} }
cost_function {
relative_pose_2d {
first { object_id: "start_node" timestamp: 1 }
second { object_id: "end_node" timestamp: 1 }
parameters {
@ -66,20 +63,16 @@ constexpr char kRelativePose2D[] = R"PROTO(
translation_weight: 1
rotation_weight: 1
}
}
}
)PROTO";
TEST(CeresOptimizerTest, SmokeTest) {
PoseGraphData data;
data.nodes.pose_2d_nodes.emplace(
NodeId{"start_node", common::FromUniversal(1)},
GetPose2D(ParseProto<proto::Node>(kStartNode)));
data.nodes.pose_2d_nodes.emplace(
NodeId{"end_node", common::FromUniversal(1)},
GetPose2D(ParseProto<proto::Node>(kEndNode)));
data.constraints.emplace_back(absl::make_unique<RelativePoseConstraint2D>(
"constraint_1", ParseProto<proto::LossFunction>(R"(quadratic_loss: {})"),
ParseProto<proto::RelativePose2D>(kRelativePose2D)));
AddNodeToPoseGraphData(ParseProto<proto::Node>(kStartNode), &data);
AddNodeToPoseGraphData(ParseProto<proto::Node>(kEndNode), &data);
AddConstraintToPoseGraphData(ParseProto<proto::Constraint>(kRelativePose2D),
&data);
CeresOptimizer optimizer(ceres::Solver::Options{});
EXPECT_EQ(optimizer.Solve(&data), Optimizer::SolverStatus::CONVERGENCE);
}

38
cartographer/pose_graph/pose_graph_controller.cc Normal file
View File

@ -0,0 +1,38 @@
/*
* 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_graph_controller.h"
namespace cartographer {
namespace pose_graph {
void PoseGraphController::AddNode(const proto::Node& node) {
common::MutexLocker locker(&mutex_);
AddNodeToPoseGraphData(node, &data_);
}
void PoseGraphController::AddConstraint(const proto::Constraint& constraint) {
common::MutexLocker locker(&mutex_);
AddConstraintToPoseGraphData(constraint, &data_);
}
Optimizer::SolverStatus PoseGraphController::Optimize() {
common::MutexLocker locker(&mutex_);
return optimizer_->Solve(&data_);
}
} // namespace pose_graph
} // namespace cartographer

49
cartographer/pose_graph/pose_graph_controller.h Normal file
View File

@ -0,0 +1,49 @@
/*
* 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_GRAPH_CONTROLLER_H_
#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_
#include "cartographer/common/mutex.h"
#include "cartographer/pose_graph/optimizer/optimizer.h"
#include "cartographer/pose_graph/pose_graph_data.h"
namespace cartographer {
namespace pose_graph {
class PoseGraphController {
PoseGraphController(std::unique_ptr<Optimizer> optimizer)
: optimizer_(std::move(optimizer)) {}
PoseGraphController(const PoseGraphController&) = delete;
PoseGraphController& operator=(const PoseGraphController&) = delete;
void AddNode(const proto::Node& node) EXCLUDES(mutex_);
void AddConstraint(const proto::Constraint& constraint) EXCLUDES(mutex_);
Optimizer::SolverStatus Optimize() EXCLUDES(mutex_);
private:
std::unique_ptr<Optimizer> optimizer_;
mutable common::Mutex mutex_;
PoseGraphData data_ GUARDED_BY(mutex_);
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_

97
cartographer/pose_graph/pose_graph_data.cc Normal file
View File

@ -0,0 +1,97 @@
/*
* 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_graph_data.h"
#include "absl/memory/memory.h"
#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h"
#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h"
#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h"
#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h"
#include "cartographer/pose_graph/constraint/relative_pose_constraint_3d.h"
#include "cartographer/pose_graph/constraint/rotation_constraint_3d.h"
namespace cartographer {
namespace pose_graph {
namespace {
std::unique_ptr<Constraint> CreateConstraint(
const proto::Constraint& constraint) {
const auto& id = constraint.id();
const auto& loss = constraint.loss_function();
const auto& cost = constraint.cost_function();
switch (cost.type_case()) {
case (proto::CostFunction::kRelativePose2D):
return absl::make_unique<RelativePoseConstraint2D>(
id, loss, cost.relative_pose_2d());
case (proto::CostFunction::kRelativePose3D):
return absl::make_unique<RelativePoseConstraint3D>(
id, loss, cost.relative_pose_3d());
case (proto::CostFunction::kAcceleration3D):
return absl::make_unique<AccelerationConstraint3D>(
id, loss, cost.acceleration_3d());
case (proto::CostFunction::kRotation3D):
return absl::make_unique<RotationContraint3D>(id, loss,
cost.rotation_3d());
case (proto::CostFunction::kInterpolatedRelativePose2D):
return absl::make_unique<InterpolatedRelativePoseConstraint2D>(
id, loss, cost.interpolated_relative_pose_2d());
case (proto::CostFunction::kInterpolatedRelativePose3D):
return absl::make_unique<InterpolatedRelativePoseConstraint3D>(
id, loss, cost.interpolated_relative_pose_3d());
case (proto::CostFunction::TYPE_NOT_SET):
LOG(FATAL) << "Constraint cost function type is not set.";
}
return nullptr;
}
} // namespace
void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data) {
NodeId node_id(node.id());
switch (node.parameters().type_case()) {
case (proto::Parameters::kPose2D): {
data->nodes.pose_2d_nodes.emplace(
node_id,
Pose2D(node_id, node.constant(), node.parameters().pose_2d()));
return;
}
case (proto::Parameters::kPose3D): {
data->nodes.pose_3d_nodes.emplace(
node_id,
Pose3D(node_id, node.constant(), node.parameters().pose_3d()));
return;
}
case (proto::Parameters::kImuCalibration): {
data->nodes.imu_calibration_nodes.emplace(
node_id, ImuCalibration(node_id, node.constant(),
node.parameters().imu_calibration()));
return;
}
case (proto::Parameters::TYPE_NOT_SET): {
LOG(FATAL) << "Node parameter type is not set.";
return;
}
}
}
void AddConstraintToPoseGraphData(const proto::Constraint& constraint,
PoseGraphData* data) {
data->constraints.emplace_back(CreateConstraint(constraint));
}
} // namespace pose_graph
} // namespace cartographer

4
cartographer/pose_graph/pose_graph_data.h
View File

@ -28,6 +28,10 @@ struct PoseGraphData {
std::vector<std::unique_ptr<Constraint>> constraints;
};
void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data);
void AddConstraintToPoseGraphData(const proto::Constraint& constraint,
PoseGraphData* data);
} // namespace pose_graph
} // namespace cartographer

2
cartographer/pose_graph/proto/cost_function.proto
View File

@ -103,7 +103,7 @@ message InterpolatedRelativePose3D {
}
message CostFunction {
oneof Type {
oneof type {
RelativePose2D relative_pose_2d = 1;
RelativePose3D relative_pose_3d = 2;
Acceleration3D acceleration_3d = 3;

2
cartographer/pose_graph/proto/node.proto
View File

@ -48,7 +48,7 @@ message NodeId {
}
message Parameters {
oneof Type {
oneof type {
Pose2D pose_2d = 1;
Pose3D pose_3d = 2;
ImuCalibration imu_calibration = 3;