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

[GenericPoseGraph] Add parameterization. (#1385)

master
Alexander Belyaev 2018-08-14 14:37:27 +02:00 committed by Wally B. Feed
parent 05f2c6caed
commit 5261c90c34
15 changed files with 212 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/common/utils.h
View File

@ -14,6 +14,9 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_UTILS_H_
#define CARTOGRAPHER_COMMON_UTILS_H_
namespace cartographer {
namespace common {
@ -27,3 +30,5 @@ ValueType* FindOrNull(MapType& map, const KeyType& key) {
} // namespace common
} // namespace cartographer
#endif // CARTOGRAPHER_COMMON_UTILS_H_

36
cartographer/pose_graph/constraint/constraint_utils.cc
View File

@ -1,3 +1,19 @@
/*
* 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/constraint/constraint_utils.h"
namespace cartographer {
@ -14,20 +30,26 @@ void AddPose2D(Pose2D* pose, ceres::Problem* problem) {
void AddPose3D(Pose3D* pose, ceres::Problem* problem) {
auto transation = pose->mutable_translation();
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(transation->data(), transation->size());
problem->AddParameterBlock(rotation->data(), rotation->size());
problem->AddParameterBlock(transation->data(), transation->size(),
pose->translation_parameterization());
problem->AddParameterBlock(rotation->data(), rotation->size(),
pose->rotation_parameterization());
if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data());
}
}
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem) {
auto gravity = pose->mutable_gravity_constant();
auto orientation = pose->mutable_orientation();
void AddImuCalibration(ImuCalibration* imu_calibration,
ceres::Problem* problem) {
auto gravity = imu_calibration->mutable_gravity_constant();
auto orientation = imu_calibration->mutable_orientation();
problem->AddParameterBlock(gravity, 1);
problem->AddParameterBlock(orientation->data(), orientation->size());
if (pose->constant()) {
problem->AddParameterBlock(orientation->data(), orientation->size(),
imu_calibration->orientation_parameterization());
if (imu_calibration->constant()) {
problem->SetParameterBlockConstant(gravity);
problem->SetParameterBlockConstant(orientation->data());
}

11
cartographer/pose_graph/constraint/constraint_utils.h
View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/node/imu_calibration.h"
#include "cartographer/pose_graph/node/pose_2d.h"
#include "cartographer/pose_graph/node/pose_3d.h"
@ -31,8 +32,16 @@ void AddPose3D(Pose3D* pose, ceres::Problem* problem);
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem);
template <typename MapType,
typename UniquePtrType = typename MapType::mapped_type,
typename ValueTyper = typename UniquePtrType::element_type>
ValueTyper* FindNodeOrNull(MapType& map, const NodeId& node_id) {
auto node = common::FindOrNull(map, node_id);
return node == nullptr ? nullptr : node->get();
}
#define FIND_NODE_OR_RETURN(node_name, node_id, map, log_message) \
auto node_name = common::FindOrNull(map, node_id); \
auto node_name = FindNodeOrNull(map, node_id); \
if (node_name == nullptr) { \
LOG(INFO) << log_message; \
return; \

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

@ -20,13 +20,12 @@ namespace cartographer {
namespace pose_graph {
ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant,
const proto::ImuCalibration& imu_calibration)
const proto::ImuCalibration& imu)
: Node(node_id, constant),
gravity_constant_(imu_calibration.gravity_constant()),
orientation_{{imu_calibration.orientation().x(),
imu_calibration.orientation().y(),
imu_calibration.orientation().z(),
imu_calibration.orientation().w()}} {}
gravity_constant_(imu.gravity_constant()),
orientation_{{imu.orientation().x(), imu.orientation().y(),
imu.orientation().z(), imu.orientation().w()}},
orientation_parameterization_(imu.orientation_parameterization()) {}
proto::Parameters ImuCalibration::ToParametersProto() const {
proto::Parameters parameters;
@ -41,6 +40,8 @@ proto::Parameters ImuCalibration::ToParametersProto() const {
orientation->set_y(orientation_[1]);
orientation->set_z(orientation_[2]);
orientation->set_w(orientation_[3]);
imu_calibration->set_orientation_parameterization(
orientation_parameterization_.ToProto());
return parameters;
}

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

@ -21,6 +21,7 @@
#include <array>
#include "cartographer/pose_graph/node/parameterization/parameterization.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
@ -37,12 +38,17 @@ class ImuCalibration : public Node {
std::array<double, 4>* mutable_orientation() { return &orientation_; }
const std::array<double, 4>& orientation() const { return orientation_; }
ceres::LocalParameterization* orientation_parameterization() const {
return orientation_parameterization_.ceres_parameterization();
}
protected:
proto::Parameters ToParametersProto() const final;
private:
double gravity_constant_;
std::array<double, 4> orientation_;
Parameterization orientation_parameterization_;
};
} // namespace pose_graph

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

@ -31,6 +31,7 @@ constexpr char kExpectedNode[] = R"PROTO(
imu_calibration {
gravity_constant: 10
orientation: { w: 0 x: 1 y: 2 z: 3 }
orientation_parameterization: YAW_ONLY
}
}
)PROTO";
@ -40,6 +41,7 @@ TEST(Pose3DTest, ToProto) {
true, ParseProto<proto::ImuCalibration>(R"(
gravity_constant: 10
orientation: { w: 0 x: 1 y: 2 z: 3 }
orientation_parameterization: YAW_ONLY
)"));
EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode));
}

8
cartographer/pose_graph/node/nodes.h
View File

@ -27,10 +27,10 @@ namespace cartographer {
namespace pose_graph {
struct Nodes {
// TODO(pifon): Should it really be an std::map or smth else?
std::map<NodeId, Pose2D> pose_2d_nodes;
std::map<NodeId, Pose3D> pose_3d_nodes;
std::map<NodeId, ImuCalibration> imu_calibration_nodes;
// TODO(pifon): Migrate to Swiss Tables when they are out.
std::map<NodeId, std::unique_ptr<Pose2D>> pose_2d_nodes;
std::map<NodeId, std::unique_ptr<Pose3D>> pose_3d_nodes;
std::map<NodeId, std::unique_ptr<ImuCalibration>> imu_calibration_nodes;
};
} // namespace pose_graph

61
cartographer/pose_graph/node/parameterization/parameterization.cc Normal file
View File

@ -0,0 +1,61 @@
/*
* 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/parameterization/parameterization.h"
#include "absl/memory/memory.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/internal/3d/rotation_parameterization.h"
#include "ceres/autodiff_local_parameterization.h"
#include "ceres/rotation.h"
namespace cartographer {
namespace pose_graph {
namespace {
using absl::make_unique;
using ceres::AutoDiffLocalParameterization;
using ceres::LocalParameterization;
// TODO(pifon): Check if the functors are compatible with our quaternions. Test!
std::unique_ptr<LocalParameterization> CeresParameterizationFromProto(
const proto::Parameterization& parameterization) {
switch (parameterization) {
case (proto::Parameterization::NONE):
return nullptr;
case (proto::Parameterization::YAW_ONLY):
return make_unique<AutoDiffLocalParameterization<
mapping::YawOnlyQuaternionPlus, 4, 1>>();
case (proto::Parameterization::YAW_CONSTANT):
return make_unique<AutoDiffLocalParameterization<
mapping::ConstantYawQuaternionPlus, 4, 1>>();
case (proto::Parameterization::FIX_Z):
return make_unique<ceres::SubsetParameterization>(
3, /* constant parameters */ std::vector<int>{2});
default:
LOG(FATAL) << "Parameterization is not known.";
}
return nullptr;
}
} // namespace
Parameterization::Parameterization(const proto::Parameterization& proto)
: proto_(proto),
ceres_parameterization_(CeresParameterizationFromProto(proto_)) {}
} // namespace pose_graph
} // namespace cartographer

44
cartographer/pose_graph/node/parameterization/parameterization.h Normal file
View File

@ -0,0 +1,44 @@
/*
* 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_PARAMETERIZATION_PARAMETERIZATION_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_
#include "cartographer/pose_graph/proto/node.pb.h"
#include "ceres/local_parameterization.h"
namespace cartographer {
namespace pose_graph {
class Parameterization {
public:
explicit Parameterization(const proto::Parameterization& proto);
const proto::Parameterization& ToProto() const { return proto_; }
ceres::LocalParameterization* ceres_parameterization() const {
return ceres_parameterization_.get();
}
private:
const proto::Parameterization proto_;
const std::unique_ptr<ceres::LocalParameterization> ceres_parameterization_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_

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

@ -24,8 +24,10 @@ Pose3D::Pose3D(const NodeId& node_id, bool constant,
: Node(node_id, constant),
translation_{{pose_3d.translation().x(), pose_3d.translation().y(),
pose_3d.translation().z()}},
translation_parameterization_(pose_3d.translation_parameterization()),
rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(),
pose_3d.rotation().z(), pose_3d.rotation().w()}} {}
pose_3d.rotation().z(), pose_3d.rotation().w()}},
rotation_parameterization_(pose_3d.rotation_parameterization()) {}
proto::Parameters Pose3D::ToParametersProto() const {
proto::Parameters parameters;
@ -35,12 +37,15 @@ proto::Parameters Pose3D::ToParametersProto() const {
translation->set_x(translation_[0]);
translation->set_y(translation_[1]);
translation->set_z(translation_[2]);
pose_3d->set_translation_parameterization(
translation_parameterization_.ToProto());
auto* rotation = pose_3d->mutable_rotation();
rotation->set_x(rotation_[0]);
rotation->set_y(rotation_[1]);
rotation->set_z(rotation_[2]);
rotation->set_w(rotation_[3]);
pose_3d->set_rotation_parameterization(rotation_parameterization_.ToProto());
return parameters;
}

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

@ -21,6 +21,7 @@
#include <array>
#include "cartographer/pose_graph/node/parameterization/parameterization.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
@ -32,16 +33,25 @@ class Pose3D : public Node {
std::array<double, 3>* mutable_translation() { return &translation_; }
const std::array<double, 3>& translation() const { return translation_; }
ceres::LocalParameterization* translation_parameterization() const {
return translation_parameterization_.ceres_parameterization();
}
std::array<double, 4>* mutable_rotation() { return &rotation_; }
const std::array<double, 4>& rotation() const { return rotation_; }
ceres::LocalParameterization* rotation_parameterization() const {
return rotation_parameterization_.ceres_parameterization();
}
protected:
proto::Parameters ToParametersProto() const final;
private:
std::array<double, 3> translation_;
Parameterization translation_parameterization_;
std::array<double, 4> rotation_;
Parameterization rotation_parameterization_;
};
} // namespace pose_graph

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

@ -30,6 +30,7 @@ constexpr char kExpectedNode[] = R"PROTO(
parameters {
pose_3d {
translation { x: 1 y: 2 z: 3 }
translation_parameterization: FIX_Z
rotation: { w: 0 x: 1 y: 2 z: 3 }
}
}
@ -39,7 +40,9 @@ TEST(Pose3DTest, ToProto) {
Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true,
ParseProto<proto::Pose3D>(R"(
translation { x: 1 y: 2 z: 3 }
translation_parameterization: FIX_Z
rotation: { w: 0 x: 1 y: 2 z: 3 }
rotation_parameterization: NONE
)"));
EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode));
}

2
cartographer/pose_graph/optimizer/ceres_optimizer.cc
View File

@ -26,6 +26,8 @@ ceres::Problem::Options CreateCeresProblemOptions() {
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
problem_options.loss_function_ownership =
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
problem_options.local_parameterization_ownership =
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
return problem_options;
}

35
cartographer/pose_graph/pose_graph_data.cc
View File

@ -28,6 +28,8 @@ namespace cartographer {
namespace pose_graph {
namespace {
using absl::make_unique;
std::unique_ptr<Constraint> CreateConstraint(
const proto::Constraint& constraint) {
const auto& id = constraint.id();
@ -35,22 +37,21 @@ std::unique_ptr<Constraint> CreateConstraint(
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());
return make_unique<RelativePoseConstraint2D>(id, loss,
cost.relative_pose_2d());
case (proto::CostFunction::kRelativePose3D):
return absl::make_unique<RelativePoseConstraint3D>(
id, loss, cost.relative_pose_3d());
return make_unique<RelativePoseConstraint3D>(id, loss,
cost.relative_pose_3d());
case (proto::CostFunction::kAcceleration3D):
return absl::make_unique<AccelerationConstraint3D>(
id, loss, cost.acceleration_3d());
return make_unique<AccelerationConstraint3D>(id, loss,
cost.acceleration_3d());
case (proto::CostFunction::kRotation3D):
return absl::make_unique<RotationContraint3D>(id, loss,
cost.rotation_3d());
return make_unique<RotationContraint3D>(id, loss, cost.rotation_3d());
case (proto::CostFunction::kInterpolatedRelativePose2D):
return absl::make_unique<InterpolatedRelativePoseConstraint2D>(
return make_unique<InterpolatedRelativePoseConstraint2D>(
id, loss, cost.interpolated_relative_pose_2d());
case (proto::CostFunction::kInterpolatedRelativePose3D):
return absl::make_unique<InterpolatedRelativePoseConstraint3D>(
return 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.";
@ -65,20 +66,22 @@ void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data) {
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()));
node_id, make_unique<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()));
node_id, make_unique<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()));
node_id,
make_unique<ImuCalibration>(node_id, node.constant(),
node.parameters().imu_calibration()));
return;
}
case (proto::Parameters::TYPE_NOT_SET): {

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

@ -20,8 +20,9 @@ import "cartographer/transform/proto/transform.proto";
enum Parameterization {
NONE = 0;
YAW_ONLY = 1;
YAW_CONSTANT = 2;
FIX_Z = 1;
YAW_ONLY = 2;
YAW_CONSTANT = 3;
}
message Pose2D {
@ -39,7 +40,7 @@ message Pose3D {
message ImuCalibration {
double gravity_constant = 1;
transform.proto.Quaterniond orientation = 2;
Parameterization rotation_parameterization = 3;
Parameterization orientation_parameterization = 3;
}
message NodeId {